diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt new file mode 100644 index 0000000000000..554f65227e354 --- /dev/null +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -0,0 +1,325 @@ +cmake_minimum_required(VERSION 3.5) +project(behavior_velocity_planner) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +# Find the non-obvious packages manually +find_package(ament_cmake REQUIRED) +find_package(Boost REQUIRED) +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(OpenCV REQUIRED) + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set(BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES + autoware_api_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_planning_msgs + autoware_utils + Boost + Eigen3 + diagnostic_msgs + geometry_msgs + lanelet2_extension + PCL + pcl_conversions + rclcpp + sensor_msgs + interpolation + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + vehicle_info_util + visualization_msgs + nav_msgs + grid_map_ros +) + + +# Common +ament_auto_add_library(scene_module_lib SHARED + src/utilization/path_utilization.cpp + src/utilization/util.cpp + src/utilization/interpolate.cpp +) + +target_include_directories(scene_module_lib + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_lib ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + + +# Scene Module: Stop Line +ament_auto_add_library(scene_module_stop_line SHARED + src/scene_module/stop_line/manager.cpp + src/scene_module/stop_line/scene.cpp + src/scene_module/stop_line/debug.cpp +) + +target_include_directories(scene_module_stop_line + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_stop_line ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_stop_line scene_module_lib) + + +# Scene Module: Crosswalk +ament_auto_add_library(scene_module_crosswalk SHARED + src/scene_module/crosswalk/manager.cpp + src/scene_module/crosswalk/scene_crosswalk.cpp + src/scene_module/crosswalk/scene_walkway.cpp + src/scene_module/crosswalk/debug.cpp + src/scene_module/crosswalk/util.cpp +) + +target_include_directories(scene_module_crosswalk + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_crosswalk ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_crosswalk scene_module_lib) + + +# Scene Module: Intersection +ament_auto_add_library(scene_module_intersection SHARED + src/scene_module/intersection/manager.cpp + src/scene_module/intersection/scene_intersection.cpp + src/scene_module/intersection/scene_merge_from_private_road.cpp + src/scene_module/intersection/debug.cpp + src/scene_module/intersection/util.cpp +) + +target_include_directories(scene_module_intersection + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_intersection ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_intersection scene_module_lib) + + +# Scene Module: Traffic Light +ament_auto_add_library(scene_module_traffic_light SHARED + src/scene_module/traffic_light/manager.cpp + src/scene_module/traffic_light/scene.cpp + src/scene_module/traffic_light/debug.cpp +) + +target_include_directories(scene_module_traffic_light + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_traffic_light ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_traffic_light scene_module_lib) + + +# Scene Module: Blind Spot +ament_auto_add_library(scene_module_blind_spot SHARED + src/scene_module/blind_spot/manager.cpp + src/scene_module/blind_spot/scene.cpp + src/scene_module/blind_spot/debug.cpp +) + +target_include_directories(scene_module_blind_spot + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_blind_spot ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_blind_spot scene_module_lib) + + +# Scene Module: Detection Area +ament_auto_add_library(scene_module_detection_area SHARED + src/scene_module/detection_area/manager.cpp + src/scene_module/detection_area/scene.cpp + src/scene_module/detection_area/debug.cpp +) + +target_include_directories(scene_module_detection_area + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_detection_area ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_detection_area scene_module_lib) + +# Scene Module: No Stopping Area +ament_auto_add_library(scene_module_no_stopping_area SHARED + src/scene_module/no_stopping_area/manager.cpp + src/scene_module/no_stopping_area/scene_no_stopping_area.cpp + src/scene_module/no_stopping_area/debug.cpp +) + +target_include_directories(scene_module_no_stopping_area + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_no_stopping_area ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_no_stopping_area scene_module_lib) + + +# Scene Module: Virtual Traffic Light +ament_auto_add_library(scene_module_virtual_traffic_light SHARED + src/scene_module/virtual_traffic_light/manager.cpp + src/scene_module/virtual_traffic_light/scene.cpp + src/scene_module/virtual_traffic_light/debug.cpp +) + +target_include_directories(scene_module_virtual_traffic_light + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_virtual_traffic_light ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_virtual_traffic_light scene_module_lib) + + +# SceneModule OcclusionSpot +ament_auto_add_library(scene_module_occlusion_spot SHARED + src/scene_module/occlusion_spot/grid_utils.cpp + src/scene_module/occlusion_spot/geometry.cpp + src/scene_module/occlusion_spot/manager.cpp + src/scene_module/occlusion_spot/debug.cpp + src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp + src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp + src/scene_module/occlusion_spot/occlusion_spot_utils.cpp + src/scene_module/occlusion_spot/risk_predictive_braking.cpp +) + +target_include_directories(scene_module_occlusion_spot + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_occlusion_spot ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_occlusion_spot scene_module_lib) + +# Scene Module Manager +ament_auto_add_library(scene_module_manager SHARED + src/planner_manager.cpp +) + +target_include_directories(scene_module_manager + PUBLIC + $ + $ +) + +ament_target_dependencies(scene_module_manager ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(scene_module_manager scene_module_lib) + + +# Node +ament_auto_add_library(behavior_velocity_planner SHARED + src/node.cpp +) + +target_include_directories(behavior_velocity_planner + PUBLIC + $ + $ +) + +ament_target_dependencies(behavior_velocity_planner ${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +target_link_libraries(behavior_velocity_planner + scene_module_lib + scene_module_stop_line + scene_module_crosswalk + scene_module_intersection + scene_module_traffic_light + scene_module_blind_spot + scene_module_occlusion_spot + scene_module_detection_area + scene_module_no_stopping_area + scene_module_virtual_traffic_light + scene_module_manager +) + +ament_export_dependencies(${BEHAVIOR_VELOCITY_PLANNER_DEPENDENCIES}) + +rclcpp_components_register_node(behavior_velocity_planner + PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" + EXECUTABLE behavior_velocity_planner_node +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # utils for test + ament_auto_add_library(utilization SHARED + src/utilization/util.cpp + ) + # Gtest for utilization + ament_add_gtest(utilization-test + test/src/test_state_machine.cpp + test/src/test_arc_lane_util.cpp + test/src/test_utilization.cpp + ) + target_link_libraries(utilization-test + gtest_main + utilization + ) + + # Gtest for occlusion spot + ament_add_gtest(occlusion_spot-test + test/src/test_occlusion_spot_utils.cpp + test/src/test_risk_predictive_braking.cpp + test/src/test_geometry.cpp + test/src/test_grid_utils.cpp + test/src/test_performances.cpp + ) + target_link_libraries(occlusion_spot-test + gtest_main + scene_module_occlusion_spot + ) + +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md new file mode 100644 index 0000000000000..2786ec1ee6e22 --- /dev/null +++ b/planning/behavior_velocity_planner/README.md @@ -0,0 +1,54 @@ +# Behavior Velocity Planner + +## Overview + +`behavior_velocity_planner` is a planner that adjust velocity based on the traffic rules. +It consists of several modules. Please refer to the links listed below for detail on each module. + +- [Blind Spot](blind-spot-design.md) +- [Crosswalk](crosswalk-design.md) +- [Detection Area](detection-area-design.md) +- [Intersection](intersection-design.md) +- [Stop Line](stop-line-design.md) +- [Traffic Light](traffic-light-design.md) +- [Occlusion Spot](occlusion-spot-design.md) +- [No Stopping Area](no-stopping-area.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. + +![set_stop_velocity](./docs/set_stop_velocity.drawio.svg) + +## 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 | + +## Output topics + +| Name | Type | Description | +| ---------------------- | -------------------------------------------- | -------------------------------------- | +| `~output/path` | autoware_auto_planning_msgs::msg::Path | path to be followed | +| `~output/stop_reasons` | autoware_planning_msgs::msg::StopReasonArray | reasons that cause the vehicle to stop | + +## Node parameters + +| Parameter | Type | Description | +| ----------------------- | ------ | ----------------------------------------------------------------------------------- | +| `launch_blind_spot` | bool | whether to launch blind_spot module | +| `launch_crosswalk` | bool | whether to launch crosswalk module | +| `launch_detection_area` | bool | whether to launch detection_area module | +| `launch_intersection` | bool | whether to launch intersection module | +| `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 | +| `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 | +| `delay_response_time` | double | (to be a global parameter) delay time of the vehicle's response to control commands | diff --git a/planning/behavior_velocity_planner/blind-spot-design.md b/planning/behavior_velocity_planner/blind-spot-design.md new file mode 100644 index 0000000000000..2a6e944604dee --- /dev/null +++ b/planning/behavior_velocity_planner/blind-spot-design.md @@ -0,0 +1,85 @@ +### Blind Spot + +#### Role + +Blind spot check while turning right/left by a dynamic object information, and planning and planning of a velocity of the start/stop. + +![brief](./docs/blind_spot/blind_spot.svg) + +### Definition + +Sets a stop line, a pass judge line, a detection area and conflict area based on a map information and a self position. + +- Stop line : Automatically created based on crossing lane information. + +- Pass judge line : A position to judge if stop or not to avoid a rapid brake. + +- Detection area : Right/left side area of the self position. + +- Conflict area : Right/left side area from the self position to the stop line. + +#### Module Parameters + +| Parameter | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------------- | +| `stop_line_margin` | double | [m] a margin that the vehicle tries to stop before stop_line | +| `backward_length` | double | [m] distance from closest path point to the edge of beginning point. | +| `ignore_width_from_center_line` | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not | +| `max_future_movement_time` | double | [s] maximum time for considering future movement of object | + +#### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +if (ego is turning right or left ?) then (yes) +else (no) + stop +endif + +:calculate pass judge Line; + +if (ego vehicle is not after pass judge line?) then (yes) +else (no) + stop +endif + +:check obstacle in blind spot; + +if (obstacle found in blind spot?) then (yes) + :set current state as STOP; +else (no) + :set current state as GO; +endif + +:set state with margin time; + +if (current state is same as previous state) then (yes) + :reset timer; +else if (state is GO->STOP) then (yes) + :set state as STOP; + :reset timer; +else if (state is STOP -> GO) then (yes) + if (start time is not set) then (yes) + :set start time; + else(no) + :calculate duration; + if(duration is more than margin time)then (yes) + :set state GO; + :reset timer; + endif + endif +endif + + +if (state is STOP) then (yes) + :set stop velocity; + + :set stop reason and factor; +endif + +stop +@enduml +``` diff --git a/planning/behavior_velocity_planner/config/blind_spot.param.yaml b/planning/behavior_velocity_planner/config/blind_spot.param.yaml new file mode 100644 index 0000000000000..332e8bfb48d68 --- /dev/null +++ b/planning/behavior_velocity_planner/config/blind_spot.param.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + blind_spot: + stop_line_margin: 1.0 # [m] + backward_length: 15.0 # [m] + ignore_width_from_center_line: 0.7 # [m] + max_future_movement_time: 10.0 # [second] diff --git a/planning/behavior_velocity_planner/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/config/crosswalk.param.yaml new file mode 100644 index 0000000000000..92e8ba6801382 --- /dev/null +++ b/planning/behavior_velocity_planner/config/crosswalk.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + crosswalk: + crosswalk: + stop_line_distance: 1.5 # make stop line away from crosswalk when no explicit stop line exists + stop_margin: 1.0 + slow_margin: 2.0 + slow_velocity: 2.76 # 2.76 m/s = 10.0 kmph + stop_predicted_object_prediction_time_margin: 3.0 + + walkway: + stop_line_distance: 1.5 # make stop line away from walkway when no explicit stop line exists + stop_margin: 1.0 + stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_planner/config/detection_area.param.yaml b/planning/behavior_velocity_planner/config/detection_area.param.yaml new file mode 100644 index 0000000000000..9174045bf0150 --- /dev/null +++ b/planning/behavior_velocity_planner/config/detection_area.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + detection_area: + stop_margin: 0.0 + use_dead_line: false + dead_line_margin: 5.0 + use_pass_judge_line: false + state_clear_time: 2.0 diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml new file mode 100644 index 0000000000000..1725a4d0193db --- /dev/null +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + intersection: + state_transit_margin_time: 0.5 + decel_velocity: 8.33 # 8.33m/s = 30.0km/h + stop_line_margin: 3.0 + stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. + stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h + intersection_velocity: 2.778 # 2.778m/s = 10.0km/h + intersection_max_accel: 0.5 # m/ss + detection_area_margin: 0.5 # [m] + detection_area_length: 200.0 # [m] + min_predicted_path_confidence: 0.05 + collision_start_margin_time: 5.0 # [s] + collision_end_margin_time: 2.0 # [s] + + merge_from_private_road: + stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_planner/config/no_stopping_area.param.yaml b/planning/behavior_velocity_planner/config/no_stopping_area.param.yaml new file mode 100644 index 0000000000000..32cd05a9cc153 --- /dev/null +++ b/planning/behavior_velocity_planner/config/no_stopping_area.param.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + no_stopping_area: + state_clear_time: 2.0 # [s] time to clear stop state + stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h + stop_margin: 0.0 # [m] margin to stop line at no stopping area + dead_line_margin: 1.0 # [m] dead line offset go at no stopping area + stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area + detection_area_length: 200.0 # [m] used to create detection area polygon + stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml new file mode 100644 index 0000000000000..b45bd4e165a98 --- /dev/null +++ b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml @@ -0,0 +1,25 @@ +/**: + ros__parameters: + occlusion_spot: + pedestrian_vel: 1.0 # [m/s] assume pedestrian is dashing from occlusion at this velocity + safety_time_buffer: 0.1 # [m/s] assume pedestrian is dashing from occlusion at this velocity + threshold: + detection_area_length: 200.0 # [m] the length of path to consider occlusion spot + stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop + lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision + # road type parameters + public_road: + min_velocity: 2.78 # [m/s] minimum velocity to ignore occlusion spot + ebs_decel: -6.0 # [m/s^2] maximum deceleration to assume for emergency stops. + pbs_decel: -1.0 # [m/s^2] deceleration to assume for predictive braking stops + private_road: + min_velocity: 1.5 # [m/s] minimum velocity to ignore occlusion spot + ebs_decel: -4.0 # [m/s^2] maximum deceleration to assume for emergency stops. + pbs_decel: -0.5 # [m/s^2] deceleration to assume for predictive braking stops. + sidewalk: + min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. + slice_size: 0.5 # [m] size of sidewalk slices in both length and distance relative to the ego trajectory. + focus_range: 2.5 # [m] buffer around the ego path used to build the sidewalk area. + grid: + free_space_max: 40 # maximum value of a free space cell in the occupancy grid + occupied_min: 60 # minimum value of an occupied cell in the occupancy grid diff --git a/planning/behavior_velocity_planner/config/stop_line.param.yaml b/planning/behavior_velocity_planner/config/stop_line.param.yaml new file mode 100644 index 0000000000000..a2b5ac5d5fcd1 --- /dev/null +++ b/planning/behavior_velocity_planner/config/stop_line.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + stop_line: + stop_margin: 0.0 + stop_check_dist: 2.0 + stop_duration_sec: 1.0 diff --git a/planning/behavior_velocity_planner/config/traffic_light.param.yaml b/planning/behavior_velocity_planner/config/traffic_light.param.yaml new file mode 100644 index 0000000000000..f5db276c9ead8 --- /dev/null +++ b/planning/behavior_velocity_planner/config/traffic_light.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + traffic_light: + stop_margin: 0.0 + tl_state_timeout: 1.0 + external_tl_state_timeout: 1.0 + yellow_lamp_period: 2.75 + enable_pass_judge: true diff --git a/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml new file mode 100644 index 0000000000000..992a2f0306782 --- /dev/null +++ b/planning/behavior_velocity_planner/config/virtual_traffic_light.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + virtual_traffic_light: + max_delay_sec: 3.0 diff --git a/planning/behavior_velocity_planner/crosswalk-design.md b/planning/behavior_velocity_planner/crosswalk-design.md new file mode 100644 index 0000000000000..ab87ac1323027 --- /dev/null +++ b/planning/behavior_velocity_planner/crosswalk-design.md @@ -0,0 +1,26 @@ +### CrossWalk + +#### Role + +Judgement whether a vehicle can go into a crosswalk and plan a velocity of the start/stop. + +#### Launch Timing + +Launches when there is a crosswalk on the target lane. + +#### Module Parameters + +| Parameter | Type | Description | +| -------------------------------------------------------- | ------ | ------------------------------------------------------------------------ | +| `crosswalk/stop_line_distance` | double | [m] make stop line away from crosswalk when no explicit stop line exists | +| `crosswalk/stop_margin` | double | [m] a margin that the vehicle tries to stop before stop_line | +| `crosswalk/slow_margin` | bool | [m] a margin that the vehicle tries to slow down before stop_line | +| `crosswalk/slow_velocity` | double | [m] a slow down velocity | +| `crosswalk/stop_predicted_object_prediction_time_margin` | double | [s] time margin for decision of ego vehicle to stop or not | +| `walkway/stop_line_distance` | double | [m] make stop line away from crosswalk when no explicit stop line exists | +| `walkway/stop_margin` | double | [m] a margin that the vehicle tries to stop before walkway | +| `walkway/stop_duration_sec` | double | [s] time margin for decision of ego vehicle to stop | + +#### Flowchart + +flow chart is almost the same as stop line. diff --git a/planning/behavior_velocity_planner/detection-area-design.md b/planning/behavior_velocity_planner/detection-area-design.md new file mode 100644 index 0000000000000..15f3c991fa61d --- /dev/null +++ b/planning/behavior_velocity_planner/detection-area-design.md @@ -0,0 +1,89 @@ +### Detection Area + +#### Role + +If pointcloud is detected in a detection area defined on a map, the stop planning will be executed at the predetermined point. + +![brief](./docs/detection_area/detection_area.svg) + +#### Launch Timing + +Launches if there is a detection area on the target lane. + +### Algorithm + +1. Gets a detection area and stop line from map information and confirms if there is pointcloud in the detection area +2. Inserts stop point l[m] in front of the stop line +3. Inserts a pass judge point to a point where the vehicle can stop with a max deceleration +4. Sets velocity as zero behind the stop line when the ego-vehicle is in front of the pass judge point +5. If the ego vehicle has passed the pass judge point already, it doesn’t stop and pass through. + +#### Module Parameters + +| Parameter | Type | Description | +| --------------------- | ------ | -------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | [m] a margin that the vehicle tries to stop before stop_line | +| `use_dead_line` | bool | [-] weather to use dead line or not | +| `dead_line_margin` | double | [m] ignore threshold that vehicle behind is collide with ego vehicle or not | +| `use_pass_judge_line` | bool | [-] weather to use pass judge line or not | +| `state_clear_time` | double | [s] when the vehicle is stopping for certain time without incoming obstacle, move to STOPPED state | + +#### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +:get obstacle point cloud in detection area; + +if (no obstacle point cloud in detection area?) then (yes) +else (no) + :set last time obstacle found; +endif + +:get clear stop state duration; + +if (clear stop state duration is more than state_clear_time?) then (yes) + :set current state GO; + :reset clear stop state duration; + stop +else (no) +endif + +if (use dead line?) then (yes) + :create dead line point; + if (Is there dead line point?) then (yes) + if (Is over dead line point?) then (yes) + stop + endif + endif +endif + +:calculate stop point; + +if (stop point?) then (yes) +else (no) + stop +endif + + +if (state is not stop and ego vehicle over line?) then (yes) + stop +endif + +if (use pass judge line?) then (yes) + if (state is not STOP and not enough braking distance?) then (yes) + stop + endif +endif + +:set state STOP; + +:inset stop point; + +:append stop reason and stop factor; + +stop +@enduml +``` diff --git a/planning/behavior_velocity_planner/docs/blind_spot/blind_spot.svg b/planning/behavior_velocity_planner/docs/blind_spot/blind_spot.svg new file mode 100644 index 0000000000000..1a2ca566581ab --- /dev/null +++ b/planning/behavior_velocity_planner/docs/blind_spot/blind_spot.svg @@ -0,0 +1 @@ +
Conflict Area
Conflict Area
Detection Area
Detection Area
Pass Judge Line
Pass Judge Line
Stop Line
Stop LineViewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/detection_area/detection_area.svg b/planning/behavior_velocity_planner/docs/detection_area/detection_area.svg new file mode 100644 index 0000000000000..33bd5dd154e4f --- /dev/null +++ b/planning/behavior_velocity_planner/docs/detection_area/detection_area.svg @@ -0,0 +1,3 @@ + + +
Pass Judge Point
Pass Judge Point
Detection Area
Detection Area
Stop Line
Stop Line
Stop Point
Stop Point
I [m] baselink to front
I [m] baselink to front
Viewer does not support full SVG 1.1 diff --git a/planning/behavior_velocity_planner/docs/intersection/intersection.svg b/planning/behavior_velocity_planner/docs/intersection/intersection.svg new file mode 100644 index 0000000000000..c5f2af74b1da1 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/intersection/intersection.svg @@ -0,0 +1,3 @@ + + +
Pass Judge Line
Pass Judge Line
Stop Line
Stop Line
Driving Lane
Driving Lane

Attention lane (Yellow)

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Attention lane (Yellow)...

Stuck Vehicle

If there is a stuck vehicle on an exit place in the intersection, wait at the entrance of the intersection

Stuck Vehicle...

Front vehicle in a same lane

Objects exist in this lane become the target object to judge if the ego vehicle can enter the intersection

Front vehicle in a same lane...

Consider collisions with vehicles on the attention area

Consider collisions with vehicles on the at...
Behind Vehicle in a same lane
Behind Vehicle in a s...

stuck_vehicle_detect_dist_(5.0m)

Detection distance for the stuck vehicle

stuck_vehicle_detect_dist_(5.0m)...

Stop Line Margin

Distance between stop line and crossing lane  when stop line is generated.

Stop Line Margin...
Ignore None Attention Area Collision
Ignore None Attention Area Collis...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/no_stopping_area/NoStoppingArea.svg b/planning/behavior_velocity_planner/docs/no_stopping_area/NoStoppingArea.svg new file mode 100644 index 0000000000000..6f0b0a2346316 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/no_stopping_area/NoStoppingArea.svg @@ -0,0 +1,558 @@ + + + + + + + + + + + + +
+
+
+ Pass Judge Point +
+
+
+ Pass Judge Point + + + + + + + +
+
+
+ NoStopping Area +
+
+
+ NoStopping Area + + + + + + + + + + +
+
+
+ Stop Line +
+
+
+ Stop Line + + + + + + +
+
+
+ Stop Point +
+
+
+ Stop Point + + + + + + +
+
+
+  baselink_to_front [m]
+
+
+  baselink_to_front [m] + + + + + + + +
+
+
+ Dead Line +
+
+
+ Dead Line + + + + + + + + + +
+
+
+ Stuck Vehicle +
+
+
+ Stuck Vehicle + + + + + + +
+
+
+  obstacle_stop_max_distance[m]
+
+
+  obstacle_stop_max_distance[m] + + + + + + +
+
+
+  vehicle_length [m]
+
+
+  vehicle_length [m] + + + + + +
+
+
+ maximum stopping distance consider jerk limit [m]
+
+
+ maximum stopping distance consid... + + + + + + + + + + +
+
+
+ Pass Judge Point +
+
+
+ Pass Judge Point + + + + + + + +
+
+
+ No Stopping Area +
+
+
+ No Stopping Area + + + + + + + + + +
+
+
+ Stop Line +
+
+
+ Stop Line + + + + + + +
+
+
+ Stop Point +
+
+
+ Stop Point + + + + + + +
+
+
+  baselink_to_front [m]
+
+
+  baselink_to_front [m] + + + + + + + +
+
+
+ Dead Line +
+
+
+ Dead Line + + + + + + + + +
+
+
+ Stop Point +
+
+
+ Stop Point + + + + + + +
+
+
+ rear overhang [m]
+
+
+ rear overhang [m] + + + + + +
+
+
+ maximum stopping distance consider jerk limit [m]
+
+
+ maximum stopping distance consid... + + + + + + + + +
+
+
+ Virtual Wall
+
+
+ Virtual Wall + + + + + + + + + +
+
+
+ Stop Point +
+
+
+ Stop Point + + + + + + +
+
+
+ Stop Point Consider Traffic Rules +
+
+
+ Stop Point Consider Traffic Rules + + + + + +
+
+
+ Stop Point Consider Obstacle Stop +
+
+
+ Stop Point Consider Obstacle Stop + + + + + + +
+
+
+ margin[m] +
+
+
+ margin[m] + + + + + Viewer does not support full SVG 1.1 + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg new file mode 100644 index 0000000000000..ffa95eff81d9a --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg @@ -0,0 +1,326 @@ + + + + + + + + + + +
+
+
v [m/s]
+
+
+ + v [m/s] + + + + + + + + +
+
+
s [m]
+
+
+ + s [m] + + + + + + + + + + +
+
+
x
+
+
+ + x + + + + + + +
+
+
x
+
+
+ + x + + + + + + +
+
+
+ min velocity +
+
+
+ + min velocity + + + + + + +
+
+
+ PBS maximum allowed deceleration +
+
+
+ + PBS maximum allowed deceleration + + + + + +
+
+
+ + + - -  reference maximum velocity
+
+ + --- + occlusion spot plan velocity + +
+ + + +      safe velocity consider EBS at collision point
+      current velocity
+PBS : predictive braking system to slow down before reaching collision point
+EBS : emergency braking system to stop if obstacle rush out
+ + +
+
+
+ + - -  reference maximum velocity... + + + + + + +
+
+
x
+
+
+ + x + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
Goal
+
+
+ + Goal + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
x
+
+
+ + x + + + + + + + +Viewer does not support full SVG 1.1 + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg new file mode 100644 index 0000000000000..562aabf513b11 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg @@ -0,0 +1,3 @@ + + +

ObstacleInfo

- Information of obstacle to consider as possible collision if there is a hidden obstacle that darting out from occlusion spot.

ObstacleInfo...

Intersection

- position that ego vehicle can have a collision with hidden obstacle where ego vehicle baselink is at collision point.
Intersection...

Collision Point

- position to inset safe velocity to path.
Collision Point...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg new file mode 100644 index 0000000000000..4dbc3965f7881 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg @@ -0,0 +1,3 @@ + + +

Sidewalk Slice

create sidewalk slice polygon inside focus range
Sidewalk Slice...
focus range
focus range

Only Consider Lower Bound

considering all occlusion spot  cost very much, so consider only  longitudinally or laterally closest hidden obstacle as slow down  target.


Only Consider Lower Bound...Viewer does not support full SVG 1.1 \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg b/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg new file mode 100644 index 0000000000000..dc84043949bd2 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/set_stop_velocity.drawio.svg @@ -0,0 +1,182 @@ + + + + + + + + + + + +
+
+
+ +

+ + + stop point + + +

+ +
+
+
+ + + stop point + + + + + + + + + + + + +
+
+
+ +

+ + + v + + +

+ +
+
+
+ + + v + + + + + + + +
+
+
+ +

+ + + x + + +

+ +
+
+
+ + + x + + + + + + + +
+
+
+ +

+ + + baselink to front + + +

+ +
+
+
+ + + baselink to front + + + + + + + + + + + + + + + +
+
+
+ +

+ + + output velocity + + +

+ +
+
+
+ + + output velocity + + + + + + + + +
+
+
+ +

+ + + reference velocity + + +

+ +
+
+
+ + + reference velocity + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/planning/behavior_velocity_planner/docs/stop_line/calculate_stop_pose.drawio.svg b/planning/behavior_velocity_planner/docs/stop_line/calculate_stop_pose.drawio.svg new file mode 100644 index 0000000000000..75802719b099c --- /dev/null +++ b/planning/behavior_velocity_planner/docs/stop_line/calculate_stop_pose.drawio.svg @@ -0,0 +1,89 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ offset segment +
+
+
+ + + offset segment + + + + + + + + + +
+
+
+ offset point +
+ = stop pose +
+
+
+ + + offset point... + + + + + + + +
+
+
+ offset from segment start +
+
+
+ + + offset from segme... + + + + + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/planning/behavior_velocity_planner/docs/stop_line/find_collision_segment.drawio.svg b/planning/behavior_velocity_planner/docs/stop_line/find_collision_segment.drawio.svg new file mode 100644 index 0000000000000..cf3158ceae588 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/stop_line/find_collision_segment.drawio.svg @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line +
+
+
+ + + stop line + + + + + + + + + +
+
+
+ collision segment +
+
+
+ + + collision segment + + + + + + + + + + +
+
+
+ collision point +
+
+
+ + + collision point + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/planning/behavior_velocity_planner/docs/stop_line/find_offset_segment.drawio.svg b/planning/behavior_velocity_planner/docs/stop_line/find_offset_segment.drawio.svg new file mode 100644 index 0000000000000..6d52df4cdbf7e --- /dev/null +++ b/planning/behavior_velocity_planner/docs/stop_line/find_offset_segment.drawio.svg @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ collision point +
+
+
+ + + collision point + + + + + + + + + + + +
+
+
+ stop margin +
+
+
+ + + stop margin + + + + + + + + + +
+
+
+ base_link to front +
+
+
+ + + base_link to front + + + + + + + + + +
+
+
+ offset segment +
+
+
+ + + offset segment + + + + + + + +
+
+
+ offset from segment start +
+
+
+ + + offset from segme... + + + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/planning/behavior_velocity_planner/docs/stop_line/node_and_segment.drawio.svg b/planning/behavior_velocity_planner/docs/stop_line/node_and_segment.drawio.svg new file mode 100644 index 0000000000000..690f429c72de9 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/stop_line/node_and_segment.drawio.svg @@ -0,0 +1,268 @@ + + + + + + + + + + + + + + + + + + + + +
+
+
+ node 1 +
+
+
+ + + node 1 + + + + + + + +
+
+
+ node 2 +
+
+
+ + + node 2 + + + + + + + +
+
+
+ node 3 +
+
+
+ + + node 3 + + + + + + + +
+
+
+ node 4 +
+
+
+ + + node 4 + + + + + + + +
+
+
+ node 5 +
+
+
+ + + node 5 + + + + + + + +
+
+
+ node 6 +
+
+
+ + + node 6 + + + + + + + +
+
+
+ node 7 +
+
+
+ + + node 7 + + + + + + + +
+
+
+ segment 1 +
+
+
+ + + segment 1 + + + + + + + +
+
+
+ segment 3 +
+
+
+ + + segment 3 + + + + + + + +
+
+
+ segment 2 +
+
+
+ + + segment 2 + + + + + + + +
+
+
+ segment 4 +
+
+
+ + + segment 4 + + + + + + + +
+
+
+ segment 5 +
+
+
+ + + segment 5 + + + + + + + +
+
+
+ segment 6 +
+
+
+ + + segment 6 + + + + + + + +
+
+
+ segment(i) +
+ = node(i) + node(i+1) +
+ = all segment has two points +
+
+
+ + + segment(i)... + + + + + + + + + Viewer does not support full SVG 1.1 + + + + diff --git a/planning/behavior_velocity_planner/docs/traffic_light/traffic_light.svg b/planning/behavior_velocity_planner/docs/traffic_light/traffic_light.svg new file mode 100644 index 0000000000000..d82039a090649 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/traffic_light/traffic_light.svg @@ -0,0 +1,166 @@ + + + + + + + + + + +
+
+
+ Dead Point +
+
+
+ Dead + Point + + + + + + + + +
+
+
+ +

default parameter

+

jerk_max = -5.0 [m/sss]

+ +
+
+
+ Distance between pass judge point + and stop point is calculated by... + + + + + + + + + + + + + + +
+
+
+

+ l + [m] :base link to front +

+
+
+
+ l [m] : base link to front + + + + + + + + + +
+
+
+ Stop Line +
+
+
+ Stop + Line + + + + + +
+
+
+ Pass Judge Point +
+
+
+ Pass Judge + Point + + + +
+
+
+ Stop Point +
+
+
+ Stop + Point + + Viewer does not support full SVG + 1.1 + diff --git a/planning/behavior_velocity_planner/docs/traffic_light/traffic_light_dilemma.svg b/planning/behavior_velocity_planner/docs/traffic_light/traffic_light_dilemma.svg new file mode 100644 index 0000000000000..1307a43779730 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/traffic_light/traffic_light_dilemma.svg @@ -0,0 +1,451 @@ + + + + + + + + + + +
+
+
1
+
+
+ 1 + + + + +
+
+
2
+
+
+ 2 + + + + +
+
+
3
+
+
+ 3 + + + +
+
+
+ 0 +
+
+
+ t0 + + + +
+
+
+ 1 +
+
+
+ t1 + + + +
+
+
+ 2 +
+
+
+ t2 + + + +
+
+
+ 3 +
+
+
+ t3 + + + +
+
+
+ 0 +
+
+
+ v0 + + + +
+
+
+

+ deceleration with maximum jerk +

+
+
+
+ deceleration with maxim... + + + + + + + +
+
+
+ 0 +
+
+
+ t0 + + + +
+
+
+ +
+
+
+ x + + + +
+
+
+ 0 +
+
+
+ v0 + + + +
+
+
+ optimal zone +
+
+
+ optimal + zone + + + +
+
+
+

+ dilemma zone +

+
+
+
+ dilemma + zone + + + +
+
+
+

+ pass judge +

+
+
+
+ pass + judge + + + + + +
+
+
+ +
+
+
+ Fig.1 + + + +
+
+
+ +
+
+
+ Fig.2 + + + + + + + + + + +
+
+
+

+ yellow lamp line +

+
+
+
+ yellow lamp + line + + + + + + + +
+
+
1
+
+
+ 1 + + + + +
+
+
2
+
+
+ 2 + + + + +
+
+
3
+
+
+ 3 + + + +
+
+
+
+
+
+ + + + +
+
+
+
+
+
+ + + + +
+
+
+
+
+
+ + + + +
+
+
+

+ deceleration with maximum brake +

+
+
+
+ deceleration with maximum... + + + +
+
+
+

+ system delay +

+
+
+
+ system delay + + Viewer does not support full SVG + 1.1 + \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp new file mode 100644 index 0000000000000..e06c98548ded4 --- /dev/null +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp @@ -0,0 +1,110 @@ +// Copyright 2019 Autoware Foundation +// +// 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 BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ + +#include "behavior_velocity_planner/planner_data.hpp" +#include "behavior_velocity_planner/planner_manager.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class BehaviorVelocityPlannerNode : public rclcpp::Node +{ +public: + explicit BehaviorVelocityPlannerNode(const rclcpp::NodeOptions & node_options); + +private: + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // subscriber + rclcpp::Subscription::SharedPtr + trigger_sub_path_with_lane_id_; + rclcpp::Subscription::SharedPtr + sub_predicted_objects_; + rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; + rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_external_crosswalk_states_; + rclcpp::Subscription::SharedPtr + sub_external_intersection_states_; + rclcpp::Subscription::SharedPtr + sub_external_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_virtual_traffic_light_states_; + rclcpp::Subscription::SharedPtr sub_occupancy_grid_; + + void onTrigger( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); + void onPredictedObjects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + void onVehicleVelocity(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onTrafficSignals( + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void onExternalTrafficSignals( + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void onExternalCrosswalkStates(const autoware_api_msgs::msg::CrosswalkStatus::ConstSharedPtr msg); + void onExternalIntersectionStates( + const autoware_api_msgs::msg::IntersectionStatus::ConstSharedPtr msg); + void onVirtualTrafficLightStates( + const autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); + void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); + + // publisher + rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; + rclcpp::Publisher::SharedPtr debug_viz_pub_; + + void publishDebugMarker(const autoware_auto_planning_msgs::msg::Path & path); + + // parameter + double forward_path_length_; + double backward_path_length_; + + // member + PlannerData planner_data_; + BehaviorVelocityPlannerManager planner_manager_; + + // function + geometry_msgs::msg::PoseStamped getCurrentPose(); + bool isDataReady(); +}; +} // namespace behavior_velocity_planner + +#endif // BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp new file mode 100644 index 0000000000000..98182288a7762 --- /dev/null +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_data.hpp @@ -0,0 +1,188 @@ +// Copyright 2019 Autoware Foundation +// +// 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 BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +class BehaviorVelocityPlannerNode; +struct PlannerData +{ + explicit PlannerData(rclcpp::Node & node) + : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + { + max_stop_acceleration_threshold = node.declare_parameter( + "max_accel", -5.0); // TODO(someone): read min_acc in velocity_controller.param.yaml? + max_stop_jerk_threshold = node.declare_parameter("max_jerk", -5.0); + delay_response_time = node.declare_parameter("delay_response_time", 0.50); + } + // tf + geometry_msgs::msg::PoseStamped current_pose; + + // msgs from callbacks that are used for data-ready + geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; + boost::optional current_accel; + static constexpr double velocity_buffer_time_sec = 10.0; + std::deque velocity_buffer; + autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + pcl::PointCloud::ConstPtr no_ground_pointcloud; + lanelet::LaneletMapPtr lanelet_map; + // occupancy grid + nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + + // other internal data + std::map traffic_light_id_map; + lanelet::traffic_rules::TrafficRulesPtr traffic_rules; + lanelet::routing::RoutingGraphPtr routing_graph; + std::shared_ptr overall_graphs; + + // external data + std::map + external_traffic_light_id_map; + boost::optional external_crosswalk_status_input; + boost::optional external_intersection_status_input; + autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states; + + // parameters + vehicle_info_util::VehicleInfo vehicle_info_; + + // additional parameters + double max_stop_acceleration_threshold; + double max_stop_jerk_threshold; + double delay_response_time; + double stop_line_extend_length; + + bool isVehicleStopped(const double stop_duration = 0.0) const + { + if (velocity_buffer.empty()) { + return false; + } + + // Get velocities within stop_duration + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + std::vector vs; + for (const auto & velocity : velocity_buffer) { + vs.push_back(velocity.twist.linear.x); + + const auto time_diff = now - velocity.header.stamp; + if (time_diff.seconds() >= stop_duration) { + break; + } + } + + // Check all velocities + constexpr double stop_velocity = 0.1; + for (const auto & v : vs) { + if (v >= stop_velocity) { + return false; + } + } + + return true; + } + + std::shared_ptr getTrafficSignal( + const int id) const + { + if (traffic_light_id_map.count(id) == 0) { + return {}; + } + return std::make_shared( + traffic_light_id_map.at(id)); + } + + std::shared_ptr + getExternalTrafficSignal(const int id) const + { + if (external_traffic_light_id_map.count(id) == 0) { + return {}; + } + return std::make_shared( + external_traffic_light_id_map.at(id)); + } + +private: + boost::optional prev_accel_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr prev_velocity_; + double accel_lowpass_gain_; + + void updateCurrentAcc() + { + current_accel = {}; + + if (!current_velocity) { + return; + } + + if (!prev_velocity_) { + prev_velocity_ = current_velocity; + return; + } + + const double dv = current_velocity->twist.linear.x - prev_velocity_->twist.linear.x; + const double dt = std::max( + (rclcpp::Time(current_velocity->header.stamp) - rclcpp::Time(prev_velocity_->header.stamp)) + .seconds(), + 1e-03); + + const double accel = dv / dt; + prev_velocity_ = current_velocity; + + if (!prev_accel_) { + prev_accel_ = accel; + return; + } + // apply lowpass filter + current_accel = accel_lowpass_gain_ * accel + (1.0 - accel_lowpass_gain_) * prev_accel_.get(); + + prev_accel_ = current_accel; + } + friend BehaviorVelocityPlannerNode; +}; +} // namespace behavior_velocity_planner + +#endif // BEHAVIOR_VELOCITY_PLANNER__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp new file mode 100644 index 0000000000000..f77735d1e349e --- /dev/null +++ b/planning/behavior_velocity_planner/include/behavior_velocity_planner/planner_manager.hpp @@ -0,0 +1,57 @@ +// Copyright 2019 Autoware Foundation +// +// 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 BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#define BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class BehaviorVelocityPlannerManager +{ +public: + void launchSceneModule( + const std::shared_ptr & scene_module_manager_ptr); + + autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); + + diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag(); + +private: + std::vector> scene_manager_ptrs_; + diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag_; +}; +} // namespace behavior_velocity_planner + +#endif // BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp new file mode 100644 index 0000000000000..850d21ebd1829 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/manager.hpp @@ -0,0 +1,46 @@ +// Copyright 2020 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__BLIND_SPOT__MANAGER_HPP_ +#define SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class BlindSpotModuleManager : public SceneModuleManagerInterface +{ +public: + explicit BlindSpotModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "blind_spot"; } + +private: + BlindSpotModule::PlannerParam planner_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__BLIND_SPOT__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp new file mode 100644 index 0000000000000..040ff1a73dd03 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -0,0 +1,261 @@ +// Copyright 2020 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__BLIND_SPOT__SCENE_HPP_ +#define SCENE_MODULE__BLIND_SPOT__SCENE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +struct BlindSpotPolygons +{ + lanelet::CompoundPolygon3d conflict_area; + lanelet::CompoundPolygon3d detection_area; +}; + +class BlindSpotModule : public SceneModuleInterface +{ +public: + enum class State { + STOP = 0, + GO, + }; + std::string toString(const State & state) + { + if (state == State::STOP) { + return "STOP"; + } else if (state == State::GO) { + return "GO"; + } else { + return "UNKNOWN"; + } + } + + enum class TurnDirection { LEFT = 0, RIGHT, INVALID }; + + /** + * @brief Manage stop-go states with safety margin time. + */ + class StateMachine + { + public: + StateMachine() + { + state_ = State::GO; + margin_time_ = 0.0; + } + void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock); + void setState(State state); + void setMarginTime(const double t); + State getState(); + + private: + State state_; //! current state + double margin_time_; //! margin time when transit to Go from Stop + std::shared_ptr start_time_; //! first time received GO when STOP state + }; + + struct DebugData + { + autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; + + geometry_msgs::msg::Pose virtual_wall_pose; + geometry_msgs::msg::Pose stop_point_pose; + geometry_msgs::msg::Pose judge_point_pose; + lanelet::CompoundPolygon3d conflict_area_for_blind_spot; + lanelet::CompoundPolygon3d detection_area_for_blind_spot; + autoware_auto_planning_msgs::msg::PathWithLaneId spline_path; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + }; + +public: + struct PlannerParam + { + double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double backward_length; //! distance[m] from closest path point to the edge of beginning point + double ignore_width_from_center_line; //! ignore width from center line from detection_area + double + max_future_movement_time; //! maximum time[second] for considering future movement of object + }; + + BlindSpotModule( + const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + /** + * @brief plan go-stop velocity at traffic crossing with collision check between reference path + * and object predicted path + */ + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + int64_t lane_id_; + TurnDirection turn_direction_; + bool has_traffic_light_; + + // Parameter + PlannerParam planner_param_; + + /** + * @brief Check obstacle is in blind spot areas. + * Condition1: Object's position is in broad blind spot area. + * Condition2: Object's predicted position is in narrow blind spot area. + * If both conditions are met, return true + * @param path path information associated with lane id + * @param objects_ptr dynamic objects + * @param closest_idx closest path point index from ego car in path points + * @return true when an object is detected in blind spot + */ + bool checkObstacleInBlindSpot( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const; + + /** + * @brief Create half lanelet + * @param lanelet input lanelet + * @return Half lanelet + */ + lanelet::ConstLanelet generateHalfLanelet(const lanelet::ConstLanelet lanelet) const; + + /** + * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. + * Broad area is made from backward expanded point to stop line point + * @param path path information associated with lane id + * @param closest_idx closest path point index from ego car in path points + * @return Blind spot polygons + */ + boost::optional generateBlindSpotPolygons( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & pose) const; + + /** + * @brief Get vehicle edge + * @param vehicle_pose pose of ego vehicle + * @param vehicle_width width of ego vehicle + * @param base_link2front length between base link and front of ego vehicle + * @return edge of ego vehicle + */ + lanelet::LineString2d getVehicleEdge( + const geometry_msgs::msg::Pose & vehicle_pose, const double vehicle_width, + const double base_link2front) const; + + /** + * @brief Check if object is belong to targeted classes + * @param object Dynamic object + * @return True when object belong to targeted classes + */ + bool isTargetObjectType(const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief Check if at least one of object's predicted position is in area + * @param object Dynamic object + * @param area Area defined by polygon + * @return True when at least one of object's predicted position is in area + */ + bool isPredictedPathInArea( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const lanelet::CompoundPolygon3d & area) const; + + /** + * @brief Generate a stop line and insert it into the path. + * A stop line is at an intersection point of straight path with vehicle path + * @param detection_areas used to generate stop line + * @param path ego-car lane + * @param stop_line_idx generated stop line index + * @param pass_judge_line_idx generated pass judge line index + * @return false when generation failed + */ + bool generateStopLine( + const lanelet::ConstLanelets straight_lanelets, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx, + int * pass_judge_line_idx) const; + + /** + * @brief Insert a point to target path + * @param insert_idx_ip insert point index in path_ip + * @param path_ip interpolated path + * @param path target path for inserting a point + * @return inserted point idx in target path, return -1 when could not find valid index + */ + int insertPoint( + const int insert_idx_ip, const autoware_auto_planning_msgs::msg::PathWithLaneId path_ip, + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; + + /** + * @brief Calculate first path index that is conflicting lanelets. + * @param path target path + * @param lanelets target lanelets + * @return path point index + */ + boost::optional getFirstPointConflictingLanelets( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstLanelets & lanelets) const; + + /** + * @brief Get start point from lanelet + * @param lane_id lane id of objective lanelet + * @return end point of lanelet + */ + boost::optional getStartPointFromLaneLet(const int lane_id) const; + + /** + * @brief get straight lanelets in intersection + */ + lanelet::ConstLanelets getStraightLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id); + + /** + * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. + * @param objects_ptr target objects + * @param time_thr time threshold to cut path + */ + void cutPredictPathWithDuration( + autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, + const double time_thr) const; + + StateMachine state_machine_; //! for state + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__BLIND_SPOT__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp new file mode 100644 index 0000000000000..52ae827658331 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/manager.hpp @@ -0,0 +1,48 @@ +// Copyright 2020 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__CROSSWALK__MANAGER_HPP_ +#define SCENE_MODULE__CROSSWALK__MANAGER_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class CrosswalkModuleManager : public SceneModuleManagerInterface +{ +public: + explicit CrosswalkModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "crosswalk"; } + +private: + CrosswalkModule::PlannerParam crosswalk_planner_param_; + WalkwayModule::PlannerParam walkway_planner_param_; + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__CROSSWALK__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp new file mode 100644 index 0000000000000..bdd740db633bc --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_crosswalk.hpp @@ -0,0 +1,105 @@ +// Copyright 2020 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__CROSSWALK__SCENE_CROSSWALK_HPP_ +#define SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +class CrosswalkModule : public SceneModuleInterface +{ +public: + struct PlannerParam + { + double stop_margin; + double stop_line_distance; + double slow_margin; + double slow_velocity; + double stop_predicted_object_prediction_time_margin; + double slowdown_predicted_object_prediction_time_margin; + double external_input_timeout; + }; + + CrosswalkModule( + const int64_t module_id, const lanelet::ConstLanelet & crosswalk, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + int64_t module_id_; + + bool checkSlowArea( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const boost::geometry::model::polygon> & polygon, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & objects_ptr, + const pcl::PointCloud::ConstPtr & no_ground_pointcloud_ptr, + autoware_auto_planning_msgs::msg::PathWithLaneId & output); + + bool checkStopArea( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const boost::geometry::model::polygon> & polygon, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & objects_ptr, + const pcl::PointCloud::ConstPtr & no_ground_pointcloud_ptr, + autoware_auto_planning_msgs::msg::PathWithLaneId & output, bool * insert_stop); + + bool createVehiclePathPolygonInCrosswalk( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const boost::geometry::model::polygon> & + crosswalk_polygon, + const float extended_width, + boost::geometry::model::polygon> & path_polygon); + bool isTargetType(const autoware_auto_perception_msgs::msg::PredictedObject & obj); + bool isTargetExternalInputStatus(const int target_status); + + enum class State { APPROACH, INSIDE, GO_OUT }; + + lanelet::ConstLanelet crosswalk_; + State state_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__CROSSWALK__SCENE_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp new file mode 100644 index 0000000000000..44a13fa8f2878 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/scene_walkway.hpp @@ -0,0 +1,71 @@ +// Copyright 2020 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__CROSSWALK__SCENE_WALKWAY_HPP_ +#define SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class WalkwayModule : public SceneModuleInterface +{ +public: +public: + struct PlannerParam + { + double stop_margin; + double stop_line_distance; + double stop_duration_sec; + double external_input_timeout; + }; + WalkwayModule( + const int64_t module_id, const lanelet::ConstLanelet & walkway, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + int64_t module_id_; + + enum class State { APPROACH, STOP, SURPASSED }; + + lanelet::ConstLanelet walkway_; + State state_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__CROSSWALK__SCENE_WALKWAY_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp b/planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp new file mode 100644 index 0000000000000..ecbe3e5f579e9 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/crosswalk/util.hpp @@ -0,0 +1,78 @@ +// Copyright 2020 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__CROSSWALK__UTIL_HPP_ +#define SCENE_MODULE__CROSSWALK__UTIL_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "behavior_velocity_planner/planner_data.hpp" + +#include +#include + +#include + +namespace behavior_velocity_planner +{ +struct DebugData +{ + double base_link2front; + std::vector collision_points; + geometry_msgs::msg::Pose first_stop_pose; + std::vector stop_poses; + std::vector slow_poses; + std::vector> collision_lines; + std::vector> crosswalk_polygons; + std::vector> stop_polygons; + std::vector stop_factor_points; + std::vector> slow_polygons; + geometry_msgs::msg::Point nearest_collision_point; + double stop_judge_range; +}; + +bool insertTargetVelocityPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const boost::geometry::model::polygon> & polygon, + const double & margin, const double & velocity, const PlannerData & planner_data, + autoware_auto_planning_msgs::msg::PathWithLaneId & output, DebugData & debug_data, + boost::optional & first_stop_path_point_index); + +lanelet::Optional getStopLineFromMap( + const int lane_id, const std::shared_ptr & planner_data, + const std::string & attribute_name); + +bool insertTargetVelocityPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const lanelet::ConstLineString3d & stop_line, const double & margin, const double & velocity, + const PlannerData & planner_data, autoware_auto_planning_msgs::msg::PathWithLaneId & output, + DebugData & debug_data, boost::optional & first_stop_path_point_index); + +bool isClockWise( + const boost::geometry::model::polygon> & polygon); + +boost::geometry::model::polygon> inverseClockWise( + const boost::geometry::model::polygon> & polygon); +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__CROSSWALK__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp new file mode 100644 index 0000000000000..12046f1737cbe --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/manager.hpp @@ -0,0 +1,45 @@ +// Copyright 2020 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__DETECTION_AREA__MANAGER_HPP_ +#define SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class DetectionAreaModuleManager : public SceneModuleManagerInterface +{ +public: + explicit DetectionAreaModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "detection_area"; } + +private: + DetectionAreaModule::PlannerParam planner_param_; + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__DETECTION_AREA__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp new file mode 100644 index 0000000000000..43d8ae6f3c21a --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -0,0 +1,112 @@ +// Copyright 2020 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__DETECTION_AREA__SCENE_HPP_ +#define SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ + +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +class DetectionAreaModule : public SceneModuleInterface +{ +public: + enum class State { GO, STOP }; + + struct DebugData + { + double base_link2front; + std::vector stop_poses; + std::vector dead_line_poses; + geometry_msgs::msg::Pose first_stop_pose; + std::vector obstacle_points; + }; + + struct PlannerParam + { + double stop_margin; + bool use_dead_line; + double dead_line_margin; + bool use_pass_judge_line; + double state_clear_time; + }; + +public: + DetectionAreaModule( + const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + LineString2d getStopLineGeometry2d() const; + + std::vector getObstaclePoints() const; + + bool canClearStopState() const; + + bool isOverLine( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + + bool hasEnoughBrakingDistance( + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + + autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const PathIndexWithPose & stop_point) const; + + boost::optional createTargetPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin) const; + + // Key Feature + const lanelet::autoware::DetectionArea & detection_area_reg_elem_; + + // State + State state_; + std::shared_ptr last_obstacle_found_time_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__DETECTION_AREA__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp new file mode 100644 index 0000000000000..87341651cc88d --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -0,0 +1,49 @@ +// Copyright 2020 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__INTERSECTION__MANAGER_HPP_ +#define SCENE_MODULE__INTERSECTION__MANAGER_HPP_ + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class IntersectionModuleManager : public SceneModuleManagerInterface +{ +public: + explicit IntersectionModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "intersection"; } + +private: + IntersectionModule::PlannerParam intersection_param_; + MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__INTERSECTION__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp new file mode 100644 index 0000000000000..8943a25e143ae --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -0,0 +1,294 @@ +// Copyright 2020 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__INTERSECTION__SCENE_INTERSECTION_HPP_ +#define SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +// first: time, second: distance +using TimeDistanceArray = std::vector>; + +class IntersectionModule : public SceneModuleInterface +{ +public: + enum class State { + STOP = 0, + GO, + }; + std::string toString(const State & state) + { + if (state == State::STOP) { + return "STOP"; + } else if (state == State::GO) { + return "GO"; + } else { + return "UNKNOWN"; + } + } + + /** + * @brief Manage stop-go states with safety margin time. + */ + class StateMachine + { + public: + StateMachine() + { + state_ = State::GO; + margin_time_ = 0.0; + } + void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock); + void setState(State state); + void setMarginTime(const double t); + State getState(); + + private: + State state_; //! current state + double margin_time_; //! margin time when transit to Go from Stop + std::shared_ptr start_time_; //! first time received GO when STOP state + }; + + struct DebugData + { + bool stop_required; + autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; + + geometry_msgs::msg::Pose slow_wall_pose; + geometry_msgs::msg::Pose stop_wall_pose; + geometry_msgs::msg::Pose stop_point_pose; + geometry_msgs::msg::Pose judge_point_pose; + geometry_msgs::msg::Polygon ego_lane_polygon; + geometry_msgs::msg::Polygon stuck_vehicle_detect_area; + geometry_msgs::msg::Polygon candidate_collision_ego_lane_polygon; + std::vector candidate_collision_object_polygons; + std::vector intersection_detection_lanelets; + std::vector detection_area; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + }; + +public: + struct PlannerParam + { + double state_transit_margin_time; + double decel_velocity; //! used when in straight and traffic_light lane + double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check + double + stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check + double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped + double intersection_velocity; //! used for intersection passing time + double intersection_max_acc; //! used for calculating intersection velocity + double detection_area_margin; //! used for detecting objects in detection area + double detection_area_length; //! used to create detection area polygon + double detection_area_angle_thr; //! threshold in checking the angle of detecting objects + double min_predicted_path_confidence; + //! minimum confidence value of predicted path to use for collision detection + double external_input_timeout; //! used to disable external input + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + }; + + IntersectionModule( + const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + /** + * @brief plan go-stop velocity at traffic crossing with collision check between reference path + * and object predicted path + */ + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + int64_t lane_id_; + std::string turn_direction_; + bool has_traffic_light_; + + // Parameter + PlannerParam planner_param_; + + /** + * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as + * actual collision check algorithm inside this function) + * @param lanelet_map_ptr lanelet map + * @param path ego-car lane + * @param detection_areas collision check is performed for vehicles that exist in this area + * @param detection_area_lanelet_ids angle check is performed for obstacles using this lanelet + * ids + * @param objects_ptr target objects + * @param closest_idx ego-car position index on the lane + * @return true if collision is detected + */ + bool checkCollision( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::vector & detection_areas, + const std::vector & detection_area_lanelet_ids, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const int closest_idx); + + /** + * @brief Check if there is a stopped vehicle on the ego-lane. + * @param lanelet_map_ptr lanelet map + * @param path ego-car lane + * @param closest_idx ego-car position on the lane + * @param objects_ptr target objects + * @return true if exists + */ + bool checkStuckVehicleInIntersection( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const int stop_idx, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr) const; + + /** + * @brief Calculate the polygon of the path from the ego-car position to the end of the + * intersection lanelet (+ extra distance). + * @param lanelet_map_ptr lanelet map + * @param path ego-car lane + * @param closest_idx ego-car position index on the lane + * @param extra_dist extra distance from the end point of the intersection lanelet + * @param ignore_dist ignore distance from the start point of the ego-intersection lane + * @return generated polygon + */ + Polygon2d generateEgoIntersectionLanePolygon( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const int start_idx, const double extra_dist, const double ignore_dist) const; + + /** + * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. + * @param objects_ptr target objects + * @param time_thr time threshold to cut path + */ + void cutPredictPathWithDuration( + autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, + const double time_thr) const; + + /** + * @brief Calculate time that is needed for ego-vehicle to cross the intersection. (to be updated) + * @param path ego-car lane + * @param closest_idx ego-car position index on the lane + * @param objective_lane_id lanelet id on ego-car + * @return calculated time [s] + */ + TimeDistanceArray calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const int objective_lane_id) const; + + /** + * @brief check if the object has a target type for collision check + * @param object target object + * @return true if the object has a target type + */ + bool isTargetCollisionVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief check if the object has a target type for stuck check + * @param object target object + * @return true if the object has a target type + */ + bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief convert object to footprint polygon + * @param object detected object + * @return 2d polygon of the object footprint + */ + Polygon2d toFootprintPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief convert predicted object to footprint polygon + * @param object detected object + * @param predicted_pose predicted object pose + * @return 2d polygon of the object footprint + */ + Polygon2d toPredictedFootprintPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const geometry_msgs::msg::Pose & predicted_pose) const; + + /** + * @brief Whether target autoware_api_msgs::Intersection::status is valid or not + * @param target_status target autoware_api_msgs::Intersection::status + * @return rue if the object has a target type + */ + bool isTargetExternalInputStatus(const int target_status); + + /** + * @brief Whether the given pose belongs to any target lanelet or not + * @param pose pose to be checked + * @param target_lanelet_ids id list of target lanelets + * @param thresh_angle angle threshold considered to belong to a lanelet + * @return true if the given pose belongs to any target lanelet + */ + bool checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids); + + /** + * @brief Get lanes including ego lanelet and next lanelet + * @param lanelet_map_ptr lanelet map + * @param path ego-car lane + * @return ego lanelet and next lanelet + */ + lanelet::ConstLanelets getEgoLaneWithNextLane( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const; + + /** + * @brief Calculate distance between closest path point and intersection lanelet along path + * @param lanelet_map_ptr lanelet map + * @param path ego-car lane + * @param closest_idx closest path index + * @return ego lanelet and next lanelet + */ + double calcDistanceUntilIntersectionLanelet( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) const; + + StateMachine state_machine_; //! for state + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__INTERSECTION__SCENE_INTERSECTION_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp new file mode 100644 index 0000000000000..5b1deb16e329c --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -0,0 +1,139 @@ +// Copyright 2020 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__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ +#define SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +/** + * @brief This module makes sure that vehicle will stop before entering public road from private + * road. This module is meant to be registered with intersection module, which looks at intersecting + * lanes before entering intersection + */ + +namespace behavior_velocity_planner +{ +class MergeFromPrivateRoadModule : public SceneModuleInterface +{ +public: + enum class State { + STOP = 0, + GO, + }; + std::string toString(const State & state) + { + if (state == State::STOP) { + return "STOP"; + } else if (state == State::GO) { + return "GO"; + } else { + return "UNKNOWN"; + } + } + + /** + * @brief Manage stop-go states with safety margin time. + */ + class StateMachine + { + public: + StateMachine() + { + state_ = State::GO; + margin_time_ = 0.0; + } + void setState(State state); + void setMarginTime(const double t); + State getState(); + + private: + State state_; //! current state + double margin_time_; //! margin time when transit to Go from Stop + std::shared_ptr start_time_; //! first time received GO when STOP state + }; + + struct DebugData + { + autoware_auto_planning_msgs::msg::PathWithLaneId path_raw; + + geometry_msgs::msg::Pose virtual_wall_pose; + geometry_msgs::msg::Pose stop_point_pose; + geometry_msgs::msg::Pose judge_point_pose; + geometry_msgs::msg::Polygon ego_lane_polygon; + geometry_msgs::msg::Polygon stuck_vehicle_detect_area; + std::vector intersection_detection_lanelets; + std::vector detection_area; + autoware_auto_planning_msgs::msg::PathWithLaneId spline_path; + geometry_msgs::msg::Point first_collision_point; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + }; + +public: + struct PlannerParam + { + IntersectionModule::PlannerParam intersection_param; + double stop_duration_sec; + }; + + MergeFromPrivateRoadModule( + const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + /** + * @brief plan go-stop velocity at traffic crossing with collision check between reference path + * and object predicted path + */ + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + int64_t lane_id_; + std::string turn_direction_; + bool has_traffic_light_; + + autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length); + + // Parameter + PlannerParam planner_param_; + + StateMachine state_machine_; //! for state + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__INTERSECTION__SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp new file mode 100644 index 0000000000000..2610f2e7efe30 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -0,0 +1,114 @@ +// Copyright 2020 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__INTERSECTION__UTIL_HPP_ +#define SCENE_MODULE__INTERSECTION__UTIL_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace util +{ +bool setVelocityFrom( + const size_t idx, const double vel, autoware_auto_planning_msgs::msg::PathWithLaneId * input); + +bool splineInterpolate( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger); + +int insertPoint( + const geometry_msgs::msg::Pose & in_pose, + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); + +geometry_msgs::msg::Pose getAheadPose( + const size_t start_idx, const double ahead_dist, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); +bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id); +bool hasDuplicatedPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & point, int * duplicated_point_idx); + +/** + * @brief get objective polygons for detection area + */ +bool getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const int lane_id, const IntersectionModule::PlannerParam & planner_param, + std::vector * conflicting_lanelets_result, + std::vector * objective_lanelets_result, const rclcpp::Logger logger); + +/** + * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, + * read it from the map; otherwise, generate a stop line at a position where it will not collide. + * @param detection_areas used to generate stop line + * @param original_path ego-car lane + * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car + * lane) + * @param stop_line_idx generated stop line index + * @param pass_judge_line_idx generated stop line index + * @return false when generation failed + */ +bool generateStopLine( + const int lane_id, const std::vector detection_areas, + const std::shared_ptr & planner_data, + const IntersectionModule::PlannerParam & planner_param, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, + int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger); + +/** + * @brief Calculate first path index that is in the polygon. + * @param path target path + * @param polygons target polygon + * @return path point index + */ +int getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::vector & polygons); + +/** + * @brief Get stop point from map if exists + * @param stop_pose stop point defined on map + * @return true when the stop point is defined on map. + */ +bool getStopPoseIndexFromMap( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_id, + const std::shared_ptr & planner_data, int & stop_idx_ip, int dist_thr, + const rclcpp::Logger logger); + +std::vector getPolygon3dFromLaneletsVec( + const std::vector & ll_vec, double clip_length); + +std::vector getLaneletIdsFromLaneletsVec(const std::vector & ll_vec); + +double calcArcLengthFromPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const size_t src_idx, + const size_t dst_idx); + +} // namespace util +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__INTERSECTION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp new file mode 100644 index 0000000000000..5d0088c4dd641 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/manager.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 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__NO_STOPPING_AREA__MANAGER_HPP_ +#define SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "scene_module/no_stopping_area/scene_no_stopping_area.hpp" +#include "scene_module/scene_module_interface.hpp" + +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" + +#include +#include + +namespace behavior_velocity_planner +{ +class NoStoppingAreaModuleManager : public SceneModuleManagerInterface +{ +public: + explicit NoStoppingAreaModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "no_stopping_area"; } + +private: + NoStoppingAreaModule::PlannerParam planner_param_; + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__NO_STOPPING_AREA__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp new file mode 100644 index 0000000000000..3b6411b45feca --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/no_stopping_area/scene_no_stopping_area.hpp @@ -0,0 +1,193 @@ +// Copyright 2020 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__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ +#define SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ + +#define EIGEN_MPL2_ONLY + +#include "scene_module/scene_module_interface.hpp" +#include "utilization/boost_geometry_helper.hpp" +#include "utilization/state_machine.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +class NoStoppingAreaModule : public SceneModuleInterface +{ +public: + struct DebugData + { + double base_link2front; + std::vector stop_poses; + geometry_msgs::msg::Pose first_stop_pose; + std::vector stuck_points; + geometry_msgs::msg::Polygon stuck_vehicle_detect_area; + geometry_msgs::msg::Polygon stop_line_detect_area; + }; + + struct PlannerParam + { + /** + * @brief representation of a parameter for no stopping area + * + * Ego --|<--stop line margin-->|NoStoppingArea|<--front margin-->|stuck vehicle|---> path + * Ego --|<--stop line margin-->|NoStoppingArea|<-rear_overhang-->| stop point |---> path + * + */ + double state_clear_time; //! [s] time to clear stop state + double stuck_vehicle_vel_thr; //! [m/s] Threshold of the speed to be recognized as stopped + double stop_margin; //! [m] margin to stop line at no stopping area + double dead_line_margin; //! [m] dead line to go at no stopping area + double stop_line_margin; //! [m] distance from auto-generated stopline to no_stopping_area + double detection_area_length; //! [m] used to create detection area polygon + double stuck_vehicle_front_margin; //! [m] margin from area end to forward lane + double path_expand_width; //! [m] path width to calculate the edge line for both side + }; + +public: + NoStoppingAreaModule( + const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + mutable bool pass_judged_ = false; + mutable bool is_stoppable_ = true; + StateMachine state_machine_; //! for state + + /** + * @brief check if the object has a target type for stuck check + * @param object target object + * @return true if the object has a target type + */ + bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief Check if there is a stopped vehicle in stuck vehicle detect area. + * @param poly ego focusing area polygon + * @param objects_ptr target objects + * @return true if exists + */ + bool checkStuckVehiclesInNoStoppingArea( + const Polygon2d & poly, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & + predicted_obj_arr_ptr); + + /** + * @brief Check if there is a stop line in "stop line detect area". + * @param path ego-car lane + * @param poly ego focusing area polygon + * @return true if exists + */ + bool checkStopLinesInNoStoppingArea( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); + + /** + * @brief Calculate the polygon of the path from the ego-car position to the end of the + * no stopping lanelet (+ extra distance). + * @param path ego-car lane + * @param ego_pose ego-car pose + * @param margin margin from the end point of the ego-no stopping area lane + * @param extra_dist extra distance from the end point of the no stopping area lanelet + * @return generated polygon + */ + Polygon2d generateEgoNoStoppingAreaLanePolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const; + + /** + * @brief Calculate the polygon of the path from the ego-car position to the end of the + * no stopping lanelet (+ extra distance). + * @param path ego-car lane + * @param stop_line_margin stop line margin from the stopping area lane + * @return generated stop line + */ + boost::optional getStopLineGeometry2d( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const double stop_line_margin) const; + + /** + * @brief Calculate if ego-vehicle is in front of dead line or not + * @param path original path + * @param self_pose ego-car pose + * @param line_pose stop line pose on the lane + * @return if over or not + */ + bool isOverDeadLine( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + + /** + * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit + * @param self_pose ego-car pose + * @param line_pose stop line pose on the lane + * @return is stoppable in front of no stopping area + */ + bool isStoppable( + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + + /** + * @brief insert stop point on ego path + * @param path original path + * @param stop_point stop line point on the lane + */ + void insertStopPoint( + autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + + boost::optional createTargetPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin) const; + + // Key Feature + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem_; + std::shared_ptr last_obstacle_found_time_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__NO_STOPPING_AREA__SCENE_NO_STOPPING_AREA_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp new file mode 100644 index 0000000000000..4d9d6a55f6cbd --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/geometry.hpp @@ -0,0 +1,72 @@ +// Copyright 2021 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__OCCLUSION_SPOT__GEOMETRY_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ + +#include + +#include +#include + +#include + +namespace behavior_velocity_planner +{ +namespace geometry +{ +namespace bg = boost::geometry; + +// @brief represent the range of a slice +// (either by index on the lanelet centerline or by arc coordinates) +struct SliceRange +{ + double min_length{}; + double max_length{}; + double min_distance{}; + double max_distance{}; +}; + +// @brief representation of a slice along a path +struct Slice +{ + SliceRange range{}; + lanelet::BasicPolygon2d polygon{}; +}; + +//!< @brief build slices all along the trajectory +// using the given range and desired slice length and width +void buildSlices( + std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, + const double slice_length, const double slice_width); +//!< @brief build an interpolated polygon between the given bounds +void buildInterpolatedPolygon( + lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & from_bound, + const lanelet::BasicLineString2d & to_bound, const double from_length, const double to_length, + const double from_ratio_dist, const double to_ratio_dist); +//!< @brief build sidewalk slice from path +std::vector buildSidewalkSlices( + const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset, + const double lateral_offset, const double min_size, const double lateral_max_dist); +//!< @brief calculate interpolation between a and b at distance ratio t +template +T lerp(T a, T b, double t) +{ + return a + t * (b - a); +} + +} // namespace geometry +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__OCCLUSION_SPOT__GEOMETRY_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp new file mode 100644 index 0000000000000..1601d49b5806d --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/grid_utils.hpp @@ -0,0 +1,88 @@ +// Copyright 2021 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__OCCLUSION_SPOT__GRID_UTILS_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include + +namespace behavior_velocity_planner +{ +namespace grid_utils +{ +namespace occlusion_cost_value +{ +static constexpr int NO_INFORMATION = -1; +static constexpr int FREE_SPACE = 0; +static constexpr int UNKNOWN = 50; +static constexpr int OCCUPIED = 100; +} // namespace occlusion_cost_value + +struct GridParam +{ + int free_space_max; // maximum value of a freespace cell in the occupancy grid + int occupied_min; // minimum value of an occupied cell in the occupancy grid +}; +struct OcclusionSpotSquare +{ + grid_map::Index index; // index of the anchor + grid_map::Position position; // position of the anchor + int side_size; // number of cells for each side of the square +}; +// @brief structure representing a OcclusionSpot on the OccupancyGrid +struct OcclusionSpot +{ + double distance_along_lanelet; + lanelet::ConstLanelet lanelet; + lanelet::BasicPoint2d position; +}; +//!< @brief Return true +// if the given cell is a occlusion_spot square of size min_size*min_size in the given grid +bool isOcclusionSpotSquare( + OcclusionSpotSquare & occlusion_spot, const grid_map::Matrix & grid_data, + const grid_map::Index & cell, const int side_size, const grid_map::Size & grid_size); +//!< @brief Find all occlusion spots inside the given lanelet +void findOcclusionSpots( + std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, + const lanelet::BasicPolygon2d & polygon, const double min_size); +//!< @brief Return true if the path between the two given points is free of occupied cells +bool isCollisionFree( + const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2); +//!< @brief get the corner positions of the square described by the given anchor +void getCornerPositions( + std::vector & corner_positions, const grid_map::GridMap & grid, + const OcclusionSpotSquare & occlusion_spot_square); +void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid * occupancy_grid); +void toQuantizedImage( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, cv::Mat * cv_image, const GridParam & param); +void denoiseOccupancyGridCV( + nav_msgs::msg::OccupancyGrid & occupancy_grid, grid_map::GridMap & grid_map, + const GridParam & param); +} // namespace grid_utils +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__OCCLUSION_SPOT__GRID_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp new file mode 100644 index 0000000000000..68945d2ded3b0 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/manager.hpp @@ -0,0 +1,62 @@ +// Copyright 2021 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__OCCLUSION_SPOT__MANAGER_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__MANAGER_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +class OcclusionSpotModuleManager : public SceneModuleManagerInterface +{ +public: + explicit OcclusionSpotModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "occlusion_spot"; } + +private: + enum class ModuleID { PRIVATE, PUBLIC }; + using PlannerParam = occlusion_spot_utils::PlannerParam; + + PlannerParam planner_param_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + rclcpp::Publisher::SharedPtr pub_debug_occupancy_grid_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__OCCLUSION_SPOT__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp new file mode 100644 index 0000000000000..d609d7836ec2c --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp @@ -0,0 +1,171 @@ +// Copyright 2021 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__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace occlusion_spot_utils +{ +enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN }; + +struct Sidewalk +{ + double focus_range; // [m] distance to care about occlusion spot + double slice_size; // [m] size of each slice + double min_occlusion_spot_size; // [m] minumum size to care about the occlusion spot +}; + +struct VehicleInfo +{ + double vehicle_width; // [m] vehicle_width from parameter server + double baselink_to_front; // [m] wheel_base + front_overhang +}; + +struct EgoVelocity +{ + double ebs_decel; // [m/s^2] emergency braking system deceleration + double pbs_decel; // [m/s^2] predictive braking system deceleration + double min_velocity; // [m/s] minimum allowed velocity not to stop +}; + +struct PlannerParam +{ + // parameters in yaml + double safety_time_buffer; // [s] + double detection_area_length; // [m] + double stuck_vehicle_vel; // [m/s] + double lateral_distance_thr; // [m] lateral distance threshold to consider + double pedestrian_vel; // [m/s] + + double dist_thr; // [m] + double angle_thr; // [rad] + bool show_debug_grid; // [-] + + VehicleInfo vehicle_info; + EgoVelocity private_road; + EgoVelocity public_road; + Sidewalk sidewalk; + grid_utils::GridParam grid; +}; + +struct ObstacleInfo +{ + geometry_msgs::msg::Point position; + double max_velocity; // [m/s] Maximum velocity of the possible obstacle +}; + +/** + * @brief representation of a possible collision between ego and some obstacle + * ^ + * | + * Ego ---------collision----------intersection-------> path + * | + * ------------------ | + * | Vehicle | obstacle + * ------------------ + */ +struct PossibleCollisionInfo +{ + ObstacleInfo obstacle_info; // For hidden obstacle + autoware_auto_planning_msgs::msg::PathPoint + collision_path_point; // For baselink at collision point + geometry_msgs::msg::Pose intersection_pose; // For egp path and hidden obstacle + lanelet::ArcCoordinates arc_lane_dist_at_collision; // For ego distance to obstacle in s-d + PossibleCollisionInfo() = default; + PossibleCollisionInfo( + const ObstacleInfo & obstacle_info, + const autoware_auto_planning_msgs::msg::PathPoint & collision_path_point, + const geometry_msgs::msg::Pose & intersection_pose, + const lanelet::ArcCoordinates & arc_lane_dist_to_occlusion) + : obstacle_info(obstacle_info), + collision_path_point(collision_path_point), + intersection_pose(intersection_pose), + arc_lane_dist_at_collision(arc_lane_dist_to_occlusion) + { + } +}; + +bool splineInterpolate( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger); +ROAD_TYPE getCurrentRoadType( + const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr); +//!< @brief build a Lanelet from a interpolated path +lanelet::ConstLanelet buildPathLanelet( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); +//!< @brief calculate intersection and collision point from occlusion spot +void calculateCollisionPathPointFromOcclusionSpot( + PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point, + const double offset_from_ego_to_target, const lanelet::ConstLanelet & path_lanelet, + const PlannerParam & param); +//!< @brief create hidden collision behind parked car +void createPossibleCollisionBehindParkedVehicle( + std::vector & possible_collisions, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PlannerParam & param, + const double offset_from_ego_to_target, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & dyn_obj_arr); +//!< @brief set velocity and orientation to collision point based on previous Path with laneId +void calcSlowDownPointsForPossibleCollision( + const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const double offset_from_ego_to_target, std::vector & possible_collisions); +//!< @brief extract lanelet that includes target_road_type only +bool extractTargetRoad( + const int closest_idx, const lanelet::LaneletMapPtr lanelet_map_ptr, const double max_range, + const autoware_auto_planning_msgs::msg::PathWithLaneId & src_path, + double & offset_from_closest_to_target, + autoware_auto_planning_msgs::msg::PathWithLaneId & tar_path, const ROAD_TYPE & target_road_type); +//!< @brief generate collision coming from occlusion spots of the given grid map and lanelet map +void generatePossibleCollisions( + std::vector & possible_collisions, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const grid_map::GridMap & grid, + const double offset_from_ego_to_closest, const double offset_from_closest_to_target, + const PlannerParam & param, std::vector & debug); +//!< @brief convert a set of occlusion spots found on sidewalk slice +void generateSidewalkPossibleCollisionFromOcclusionSpot( + std::vector & possible_collisions, const grid_map::GridMap & grid, + const std::vector & occlusion_spot_positions, + const double offset_form_ego_to_target, const lanelet::ConstLanelet & path_lanelet, + const PlannerParam & param); +//!< @brief generate possible collisions coming from occlusion spots on the side of the path +void generateSidewalkPossibleCollisions( + std::vector & possible_collisions, const grid_map::GridMap & grid, + const double offset_from_ego_to_closest, const double offset_from_closest_to_target, + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, + std::vector & debug); + +} // namespace occlusion_spot_utils +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__OCCLUSION_SPOT__OCCLUSION_SPOT_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp new file mode 100644 index 0000000000000..00e1d5c1ba79e --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/risk_predictive_braking.hpp @@ -0,0 +1,79 @@ +// Copyright 2021 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__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace occlusion_spot_utils +{ +void applySafeVelocityConsideringPossibleCollison( + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + std::vector & possible_collisions, const double current_vel, + const EgoVelocity & ego, const PlannerParam & param); + +int insertSafeVelocityToPath( + const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); + +// @brief calculates the maximum velocity allowing to decelerate within the given distance +inline double calculatePredictiveBrakingVelocity( + const double ego_vel, const double dist2col, const double pbs_decel) +{ + return std::sqrt(std::max(std::pow(ego_vel, 2.0) - 2.0 * std::abs(pbs_decel) * dist2col, 0.0)); +} + +/** + * @param: safety_time: safety time buffer for reaction + * @param: dist_to_obj: distance to virtual darting object + * @param: v_obs: relative velocity for virtual darting object + * @param: ebs_decel: emergency brake + * @return safe velocity considering rpb + **/ +inline double calculateSafeRPBVelocity( + const double safety_time, const double dist_to_obj, const double v_obs, const double ebs_decel) +{ + const double t_vir = dist_to_obj / v_obs; + // min safety time buffer is at least more than 0 + const double ttc_virtual = std::max(t_vir - safety_time, 0.0); + // safe velocity consider emergency brake + const double v_safe = std::abs(ebs_decel) * ttc_virtual; + return v_safe; +} + +inline double getPBSLimitedRPBVelocity( + const double pbs_vel, const double rpb_vel, const double min_vel, const double original_vel) +{ + const double max_vel_noise = 0.05; + // ensure safe velocity doesn't exceed maximum allowed pbs deceleration + double rpb_pbs_limited_vel = std::max(pbs_vel + max_vel_noise, rpb_vel); + // ensure safe path velocity is also above ego min velocity + rpb_pbs_limited_vel = std::max(rpb_pbs_limited_vel, min_vel); + // ensure we only lower the original velocity (and do not increase it) + rpb_pbs_limited_vel = std::min(rpb_pbs_limited_vel, original_vel); + return rpb_pbs_limited_vel; +} + +} // namespace occlusion_spot_utils +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__OCCLUSION_SPOT__RISK_PREDICTIVE_BRAKING_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp new file mode 100644 index 0000000000000..422341e6cb897 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.hpp @@ -0,0 +1,82 @@ +// Copyright 2021 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__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class OcclusionSpotInPrivateModule : public SceneModuleInterface +{ + using PlannerParam = occlusion_spot_utils::PlannerParam; + +public: + struct DebugData + { + double z; + std::string road_type = "private"; + std::vector sidewalks; + std::vector possible_collisions; + }; + + OcclusionSpotInPrivateModule( + const int64_t module_id, std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock, + const rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief plan occlusion spot velocity at unknown area in occupancy grid + */ + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects_array_; + rclcpp::Publisher::SharedPtr publisher_; + + // Parameter + PlannerParam param_; + +protected: + int64_t module_id_; + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PRIVATE_ROAD_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp new file mode 100644 index 0000000000000..e8e71b8364216 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp @@ -0,0 +1,80 @@ +// Copyright 2021 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__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ +#define SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class OcclusionSpotInPublicModule : public SceneModuleInterface +{ + using PlannerParam = occlusion_spot_utils::PlannerParam; + +public: + struct DebugData + { + std::string road_type = "public"; + double z; + std::vector sidewalks; + std::vector possible_collisions; + }; + + OcclusionSpotInPublicModule( + const int64_t module_id, std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + /** + * @brief plan occlusion spot velocity + */ + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects_array_; + + // Parameter + PlannerParam param_; + +protected: + int64_t module_id_; + + // Debug + mutable DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__OCCLUSION_SPOT__SCENE_OCCLUSION_SPOT_IN_PUBLIC_ROAD_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp new file mode 100644 index 0000000000000..c5121aacd6035 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/scene_module_interface.hpp @@ -0,0 +1,208 @@ +// Copyright 2020 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__SCENE_MODULE_INTERFACE_HPP_ +#define SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ + +#include "behavior_velocity_planner/planner_data.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include + +// Debug +#include + +#include + +namespace behavior_velocity_planner +{ +class SceneModuleInterface +{ +public: + explicit SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + : module_id_(module_id), logger_(logger), clock_(clock) + { + } + virtual ~SceneModuleInterface() = default; + + virtual bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) = 0; + virtual visualization_msgs::msg::MarkerArray createDebugMarkerArray() = 0; + + int64_t getModuleId() const { return module_id_; } + void setPlannerData(const std::shared_ptr & planner_data) + { + planner_data_ = planner_data; + } + + boost::optional getInfrastructureCommand() + { + return infrastructure_command_; + } + + void setInfrastructureCommand( + const boost::optional & command) + { + infrastructure_command_ = command; + } + + boost::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } + +protected: + const int64_t module_id_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr planner_data_; + boost::optional infrastructure_command_; + boost::optional first_stop_path_point_index_; +}; + +class SceneModuleManagerInterface +{ +public: + SceneModuleManagerInterface(rclcpp::Node & node, const char * module_name) + : clock_(node.get_clock()), logger_(node.get_logger()) + { + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 20); + pub_stop_reason_ = node.create_publisher( + "~/output/stop_reasons", 20); + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 20); + } + + virtual ~SceneModuleManagerInterface() = default; + + virtual const char * getModuleName() = 0; + + boost::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } + + void updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + { + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); + } + + virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) + { + visualization_msgs::msg::MarkerArray debug_marker_array; + autoware_planning_msgs::msg::StopReasonArray stop_reason_array; + stop_reason_array.header.frame_id = "map"; + stop_reason_array.header.stamp = clock_->now(); + + autoware_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + first_stop_path_point_index_ = static_cast(path->points.size()) - 1; + for (const auto & scene_module : scene_modules_) { + autoware_planning_msgs::msg::StopReason stop_reason; + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path, &stop_reason); + stop_reason_array.stop_reasons.emplace_back(stop_reason); + + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + + if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) { + first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex(); + } + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + } + + if (!stop_reason_array.stop_reasons.empty()) { + pub_stop_reason_->publish(stop_reason_array); + } + pub_infrastructure_commands_->publish(infrastructure_command_array); + pub_debug_->publish(debug_marker_array); + } + +protected: + virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + + virtual std::function &)> + getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + + void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + { + const auto isModuleExpired = getModuleExpiredFunction(path); + + // Copy container to avoid iterator corruption + // due to scene_modules_.erase() in unregisterModule() + const auto copied_scene_modules = scene_modules_; + + for (const auto & scene_module : copied_scene_modules) { + if (isModuleExpired(scene_module)) { + unregisterModule(scene_module); + } + } + } + + bool isModuleRegistered(const int64_t module_id) + { + return registered_module_id_set_.count(module_id) != 0; + } + + void registerModule(const std::shared_ptr & scene_module) + { + RCLCPP_INFO( + logger_, "register task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_modules_.insert(scene_module); + } + + void unregisterModule(const std::shared_ptr & scene_module) + { + RCLCPP_INFO( + logger_, "unregister task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.erase(scene_module->getModuleId()); + scene_modules_.erase(scene_module); + } + + std::set> scene_modules_; + std::set registered_module_id_set_; + + std::shared_ptr planner_data_; + + boost::optional first_stop_path_point_index_; + rclcpp::Clock::SharedPtr clock_; + // Debug + rclcpp::Logger logger_; + rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr pub_stop_reason_; + rclcpp::Publisher::SharedPtr + pub_infrastructure_commands_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp new file mode 100644 index 0000000000000..b2e2145966345 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/manager.hpp @@ -0,0 +1,45 @@ +// Copyright 2020 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__STOP_LINE__MANAGER_HPP_ +#define SCENE_MODULE__STOP_LINE__MANAGER_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class StopLineModuleManager : public SceneModuleManagerInterface +{ +public: + explicit StopLineModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "stop_line"; } + +private: + StopLineModule::PlannerParam planner_param_; + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__STOP_LINE__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp new file mode 100644 index 0000000000000..d190eb6fc75f1 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/stop_line/scene.hpp @@ -0,0 +1,115 @@ +// Copyright 2020 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__STOP_LINE__SCENE_HPP_ +#define SCENE_MODULE__STOP_LINE__SCENE_HPP_ + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class StopLineModule : public SceneModuleInterface +{ +public: + enum class State { APPROACH, STOPPED, START }; + + struct SegmentIndexWithPose + { + size_t index; + geometry_msgs::msg::Pose pose; + }; + + struct SegmentIndexWithPoint2d + { + size_t index; + Point2d point; + }; + + struct SegmentIndexWithOffset + { + size_t index; + double offset; + }; + + struct DebugData + { + double base_link2front; + boost::optional stop_pose; + }; + + struct PlannerParam + { + double stop_margin; + double stop_check_dist; + double stop_duration_sec; + }; + +public: + StopLineModule( + const int64_t module_id, const lanelet::ConstLineString3d & stop_line, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + int64_t module_id_; + + geometry_msgs::msg::Point getCenterOfStopLine(const lanelet::ConstLineString3d & stop_line); + + boost::optional findCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line); + + boost::optional findOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const StopLineModule::SegmentIndexWithPoint2d & collision); + + boost::optional calcStopPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const boost::optional & offset_segment); + + autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const StopLineModule::SegmentIndexWithPose & insert_index_with_pose, + autoware_planning_msgs::msg::StopReason * stop_reason); + + lanelet::ConstLineString3d stop_line_; + State state_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__STOP_LINE__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp new file mode 100644 index 0000000000000..02e9317ff6b6a --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp @@ -0,0 +1,53 @@ +// Copyright 2020 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__TRAFFIC_LIGHT__MANAGER_HPP_ +#define SCENE_MODULE__TRAFFIC_LIGHT__MANAGER_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class TrafficLightModuleManager : public SceneModuleManagerInterface +{ +public: + explicit TrafficLightModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "traffic_light"; } + + void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + +private: + TrafficLightModule::PlannerParam planner_param_; + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + // Debug + rclcpp::Publisher::SharedPtr + pub_tl_state_; + + boost::optional first_ref_stop_path_point_index_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__TRAFFIC_LIGHT__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp new file mode 100644 index 0000000000000..a40adf8310f74 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/scene.hpp @@ -0,0 +1,151 @@ +// Copyright 2020 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__TRAFFIC_LIGHT__SCENE_HPP_ +#define SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class TrafficLightModule : public SceneModuleInterface +{ +public: + enum class State { APPROACH, GO_OUT }; + enum class Input { PERCEPTION, EXTERNAL, NONE }; // EXTERNAL: FOA, V2X, etc. + + struct DebugData + { + double base_link2front; + std::vector, + autoware_auto_perception_msgs::msg::TrafficSignal>> + tl_state; + std::vector stop_poses; + geometry_msgs::msg::Pose first_stop_pose; + std::vector dead_line_poses; + std::vector traffic_light_points; + geometry_msgs::msg::Point highest_confidence_traffic_light_point; + }; + + struct PlannerParam + { + double stop_margin; + double tl_state_timeout; + double external_tl_state_timeout; + double yellow_lamp_period; + bool enable_pass_judge; + }; + +public: + TrafficLightModule( + const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + + inline autoware_auto_perception_msgs::msg::LookingTrafficSignal getTrafficSignal() const + { + return looking_tl_state_; + } + + inline State getTrafficLightModuleState() const { return state_; } + + inline boost::optional getFirstRefStopPathPointIndex() const + { + return first_ref_stop_path_point_index_; + } + +private: + bool isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + + bool isTrafficSignalStop( + const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const; + + autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, + autoware_planning_msgs::msg::StopReason * stop_reason); + + bool isPassthrough(const double & signed_arc_length) const; + + bool hasTrafficLightColor( + const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, + const uint8_t & lamp_color) const; + + bool hasTrafficLightShape( + const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, + const uint8_t & lamp_shape) const; + + bool getHighestConfidenceTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, + autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state); + + bool getExternalTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, + autoware_auto_perception_msgs::msg::TrafficSignalStamped & external_tl_state); + + bool updateTrafficSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights); + + autoware_auto_perception_msgs::msg::TrafficSignalWithJudge generateTlStateWithJudgeFromTlState( + const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const; + + // Key Feature + const lanelet::TrafficLight & traffic_light_reg_elem_; + lanelet::ConstLanelet lane_; + + // State + State state_; + + // Input + Input input_; + + // Parameter + PlannerParam planner_param_; + + // Debug + DebugData debug_data_; + + // prevent paththrough chattering + bool is_prev_state_stop_; + + boost::optional first_ref_stop_path_point_index_; + + // Traffic Light State + autoware_auto_perception_msgs::msg::LookingTrafficSignal looking_tl_state_; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__TRAFFIC_LIGHT__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp new file mode 100644 index 0000000000000..b844bc68ab260 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/manager.hpp @@ -0,0 +1,45 @@ +// Copyright 2021 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__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ +#define SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ + +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface +{ +public: + explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "virtual_traffic_light"; } + +private: + VirtualTrafficLightModule::PlannerParam planner_param_; + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp new file mode 100644 index 0000000000000..02c6ff7c462ba --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/virtual_traffic_light/scene.hpp @@ -0,0 +1,122 @@ +// Copyright 2021 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__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ +#define SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +class VirtualTrafficLightModule : public SceneModuleInterface +{ +public: + enum class State : uint8_t { + NONE = 0, + REQUESTING = 1, + PASSING = 2, + FINALIZING = 3, + FINALIZED = 4, + }; + + struct MapData + { + std::string instrument_type{}; + std::string instrument_id{}; + std::vector custom_tags{}; + autoware_utils::Point3d instrument_center{}; + boost::optional stop_line{}; + autoware_utils::LineString3d start_line{}; + std::vector end_lines{}; + }; + + struct ModuleData + { + geometry_msgs::msg::Pose head_pose{}; + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + boost::optional stop_head_pose_at_stop_line; + boost::optional stop_head_pose_at_end_line; + }; + + struct PlannerParam + { + double max_delay_sec; + }; + +public: + VirtualTrafficLightModule( + const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + +private: + const int64_t module_id_; + const lanelet::autoware::VirtualTrafficLight & reg_elem_; + const lanelet::ConstLanelet lane_; + const PlannerParam planner_param_; + State state_{State::NONE}; + autoware_v2x_msgs::msg::InfrastructureCommand command_; + MapData map_data_; + ModuleData module_data_; + + void updateInfrastructureCommand(); + + void setStopReason( + const geometry_msgs::msg::Pose & stop_pose, + autoware_planning_msgs::msg::StopReason * stop_reason); + + bool isBeforeStartLine(); + + bool isBeforeStopLine(); + + bool isAfterAnyEndLine(); + + bool isNearAnyEndLine(); + + boost::optional findCorrespondingState(); + + bool isStateTimeout(const autoware_v2x_msgs::msg::VirtualTrafficLightState & state); + + bool hasRightOfWay(const autoware_v2x_msgs::msg::VirtualTrafficLightState & state); + + void insertStopVelocityAtStopLine( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason); + + void insertStopVelocityAtEndLine( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason); +}; +} // namespace behavior_velocity_planner +#endif // SCENE_MODULE__VIRTUAL_TRAFFIC_LIGHT__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp new file mode 100644 index 0000000000000..0720234ce7d77 --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/arc_lane_util.hpp @@ -0,0 +1,188 @@ +// Copyright 2021 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 UTILIZATION__ARC_LANE_UTIL_HPP_ +#define UTILIZATION__ARC_LANE_UTIL_HPP_ + +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; +namespace arc_lane_utils +{ +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithOffset = std::pair; // front index, offset + +inline double calcSignedDistance( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) +{ + Eigen::Affine3d map2p1; + tf2::fromMsg(p1, map2p1); + const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); + return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); +} + +inline boost::optional getNearestCollisionPoint( + const LineString2d & stop_line, const LineString2d & path_segment) +{ + // Find all collision points + std::vector collision_points; + bg::intersection(stop_line, path_segment, collision_points); + if (collision_points.empty()) { + return {}; + } + + // To dist list + std::vector dist_list; + dist_list.reserve(collision_points.size()); + std::transform( + collision_points.cbegin(), collision_points.cend(), std::back_inserter(dist_list), + [&path_segment](const Point2d & collision_point) { + return bg::distance(path_segment.front(), collision_point); + }); + + // Find nearest collision point + const auto min_itr = std::min_element(dist_list.cbegin(), dist_list.cend()); + const auto min_idx = std::distance(dist_list.cbegin(), min_itr); + + return collision_points.at(min_idx); +} + +template +boost::optional findCollisionSegment( + const T & path, const LineString2d & stop_line) +{ + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point + const auto & p2 = path.points.at(i + 1).point.pose.position; // Point after collision point + + const LineString2d path_segment = {{p1.x, p1.y}, {p2.x, p2.y}}; + + const auto nearest_collision_point = getNearestCollisionPoint(stop_line, path_segment); + if (nearest_collision_point) { + return std::make_pair(i, *nearest_collision_point); + } + } + + return {}; +} + +template +boost::optional findForwardOffsetSegment( + const T & path, const size_t base_idx, const double offset_length) +{ + double sum_length = 0.0; + for (size_t i = base_idx; i < path.points.size() - 1; ++i) { + sum_length += autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + + // If it's over offset point, return front index and remain offset length + if (sum_length >= offset_length) { + return std::make_pair(i, sum_length - offset_length); + } + } + + // No enough path length + return {}; +} + +template +boost::optional findBackwardOffsetSegment( + const T & path, const size_t base_idx, const double offset_length) +{ + double sum_length = 0.0; + const auto start = static_cast(base_idx) - 1; + for (std::int32_t i = start; i >= 0; --i) { + sum_length += autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); + + // If it's over offset point, return front index and remain offset length + if (sum_length >= offset_length) { + const auto k = static_cast(i); + return std::make_pair(k, sum_length - offset_length); + } + } + + // No enough path length + return {}; +} + +template +boost::optional findOffsetSegment( + const T & path, const PathIndexWithPoint2d & collision_segment, const double offset_length) +{ + const size_t collision_idx = collision_segment.first; + const Point2d & collision_point = collision_segment.second; + const auto p_front = to_bg2d(path.points.at(collision_idx).point.pose.position); + const auto p_back = to_bg2d(path.points.at(collision_idx + 1).point.pose.position); + + if (offset_length >= 0) { + return findForwardOffsetSegment( + path, collision_idx, offset_length + bg::distance(p_front, collision_point)); + } else { + return findBackwardOffsetSegment( + path, collision_idx + 1, -offset_length + bg::distance(p_back, collision_point)); + } +} + +template +geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) +{ + const size_t offset_idx = offset_segment.first; + const double remain_offset_length = offset_segment.second; + const auto & p_front = path.points.at(offset_idx).point.pose.position; + const auto & p_back = path.points.at(offset_idx + 1).point.pose.position; + + // To Eigen point + const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); + const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); + + // Calculate interpolation ratio + const auto interpolate_ratio = remain_offset_length / (p_eigen_back - p_eigen_front).norm(); + + // Add offset to front point + const auto target_point_2d = p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); + const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); + + // Calculate orientation so that X-axis would be along the trajectory + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = target_point_2d.x(); + target_pose.position.y = target_point_2d.y(); + target_pose.position.z = interpolated_z; + const double yaw = autoware_utils::calcAzimuthAngle(p_front, p_back); + target_pose.orientation = autoware_utils::createQuaternionFromYaw(yaw); + return target_pose; +} + +} // namespace arc_lane_utils +} // namespace behavior_velocity_planner + +#endif // UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp new file mode 100644 index 0000000000000..ffbab904aaf85 --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/boost_geometry_helper.hpp @@ -0,0 +1,215 @@ +// Copyright 2020 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 UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include + +// cppcheck-suppress unknownMacro +BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + geometry_msgs::msg::Pose, double, cs::cartesian, position.x, position.y, position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + geometry_msgs::msg::PoseWithCovarianceStamped, double, cs::cartesian, pose.pose.position.x, + pose.pose.position.y, pose.pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_auto_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, + pose.position.y, pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_auto_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, + point.pose.position.x, point.pose.position.y, point.pose.position.z) +BOOST_GEOMETRY_REGISTER_POINT_3D( + autoware_auto_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, + pose.position.y, pose.position.z) + +namespace behavior_velocity_planner +{ +using Point2d = boost::geometry::model::d2::point_xy; +using Segment2d = boost::geometry::model::segment; +using LineString2d = boost::geometry::model::linestring; +using Polygon2d = + boost::geometry::model::polygon; // counter-clockwise, open + +template +Point2d to_bg2d(const T & p) +{ + return Point2d(boost::geometry::get<0>(p), boost::geometry::get<1>(p)); +} + +template +LineString2d to_bg2d(const std::vector & vec) +{ + LineString2d ps; + for (const auto & p : vec) { + ps.push_back(to_bg2d(p)); + } + return ps; +} + +inline Polygon2d linestring2polygon(const LineString2d & line_string) +{ + Polygon2d polygon; + + for (const auto & p : line_string) { + polygon.outer().push_back(p); + } + + return polygon; +} + +inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) +{ + Polygon2d polygon; + + polygon.outer().push_back(left_line.front()); + + for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) { + polygon.outer().push_back(*itr); + } + + for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { + polygon.outer().push_back(*itr); + } + + return polygon; +} + +inline Polygon2d obj2polygon( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & shape) +{ + // rename + const double x = pose.position.x; + const double y = pose.position.y; + const double h = shape.x; + const double w = shape.y; + const double yaw = tf2::getYaw(pose.orientation); + + // create base polygon + Polygon2d obj_poly; + boost::geometry::exterior_ring(obj_poly) = boost::assign::list_of(h / 2.0, w / 2.0)( + -h / 2.0, w / 2.0)(-h / 2.0, -w / 2.0)(h / 2.0, -w / 2.0)(h / 2.0, w / 2.0); + + // rotate polygon(yaw) + boost::geometry::strategy::transform::rotate_transformer + rotate(-yaw); // anti-clockwise -> :clockwise rotation + Polygon2d rotate_obj_poly; + boost::geometry::transform(obj_poly, rotate_obj_poly, rotate); + + // translate polygon(x, y) + boost::geometry::strategy::transform::translate_transformer translate(x, y); + Polygon2d translate_obj_poly; + boost::geometry::transform(rotate_obj_poly, translate_obj_poly, translate); + return translate_obj_poly; +} + +inline double calcOverlapAreaRate(const Polygon2d & target, const Polygon2d & base) +{ + /* OverlapAreaRate: common area(target && base) / target area */ + + if (boost::geometry::within(target, base)) { + // target is within base, common area = target area + return 1.0; + } + + if (!boost::geometry::intersects(target, base)) { + // target and base has not intersect area + return 0.0; + } + + // calculate intersect polygon + std::vector intersects; + boost::geometry::intersection(target, base, intersects); + + // calculate area of polygon + double intersect_area = 0.0; + for (const auto & intersect : intersects) { + intersect_area += boost::geometry::area(intersect); + } + const double target_area = boost::geometry::area(target); + // specification of boost1.65 + // common area is not intersect area + const double common_area = target_area - intersect_area; + + return common_area / target_area; +} + +inline std::vector makeSegments(const LineString2d & ls) +{ + std::vector segments; + for (size_t i = 0; i < ls.size(); ++i) { + segments.emplace_back(ls.at(i), ls.at(i + 1)); + } + return segments; +} + +inline geometry_msgs::msg::Polygon toGeomMsg(const Polygon2d & polygon) +{ + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : polygon.outer()) { + point_msg.x = p.x(); + point_msg.y = p.y(); + polygon_msg.points.push_back(point_msg); + } + return polygon_msg; +} + +inline Polygon2d toBoostPoly(const geometry_msgs::msg::Polygon & polygon) +{ + Polygon2d boost_poly; + for (const auto & point : polygon.points) { + const Point2d point2d(point.x, point.y); + boost_poly.outer().push_back(point2d); + } + return boost_poly; +} + +inline Polygon2d toBoostPoly(const lanelet::BasicPolygon2d & polygon) +{ + Polygon2d boost_poly; + for (const auto & vec : polygon) { + const Point2d point2d(vec.x(), vec.y()); + boost_poly.outer().push_back(point2d); + } + + return boost_poly; +} +} // namespace behavior_velocity_planner + +#endif // UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/interpolate.hpp b/planning/behavior_velocity_planner/include/utilization/interpolate.hpp new file mode 100644 index 0000000000000..63254de4f1168 --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/interpolate.hpp @@ -0,0 +1,49 @@ +// Copyright 2020 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 UTILIZATION__INTERPOLATE_HPP_ +#define UTILIZATION__INTERPOLATE_HPP_ + +#include +#include + +namespace behavior_velocity_planner +{ +namespace interpolation +{ +// template +// bool splineInterpolateWithFixInterval(const T & input, const double interval, T * output); + +class LinearInterpolate +{ +public: + LinearInterpolate() {} + static bool interpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index, std::vector & return_value); +}; + +/* + * helper functions + */ +bool isIncrease(const std::vector & x); +bool isValidInput( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index); +std::vector calcEuclidDist(const std::vector & x, const std::vector & y); + +} // namespace interpolation +} // namespace behavior_velocity_planner + +#endif // UTILIZATION__INTERPOLATE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/marker_helper.hpp b/planning/behavior_velocity_planner/include/utilization/marker_helper.hpp new file mode 100644 index 0000000000000..6e3725ad9f599 --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/marker_helper.hpp @@ -0,0 +1,104 @@ +// Copyright 2020 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 UTILIZATION__MARKER_HELPER_HPP_ +#define UTILIZATION__MARKER_HELPER_HPP_ + +#include + +namespace behavior_velocity_planner +{ +inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) +{ + geometry_msgs::msg::Point point; + + point.x = x; + point.y = y; + point.z = z; + + return point; +} + +inline geometry_msgs::msg::Quaternion createMarkerOrientation( + double x, double y, double z, double w) +{ + geometry_msgs::msg::Quaternion quaternion; + + quaternion.x = x; + quaternion.y = y; + quaternion.z = z; + quaternion.w = w; + + return quaternion; +} + +inline geometry_msgs::msg::Vector3 createMarkerScale(double x, double y, double z) +{ + geometry_msgs::msg::Vector3 scale; + + scale.x = x; + scale.y = y; + scale.z = z; + + return scale; +} + +inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, float a) +{ + std_msgs::msg::ColorRGBA color; + + color.r = r; + color.g = g; + color.b = b; + color.a = a; + + return color; +} + +inline visualization_msgs::msg::Marker createDefaultMarker( + const char * frame_id, const builtin_interfaces::msg::Time current_time, const char * ns, + const int32_t id, const int32_t type, const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = current_time; + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0); + + marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = createMarkerScale(1.0, 1.0, 1.0); + marker.color = color; + marker.frame_locked = true; + + return marker; +} + +inline void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + const builtin_interfaces::msg::Time current_time, + visualization_msgs::msg::MarkerArray * marker_array) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + marker_array->markers.back().header.stamp = current_time; + } +} +} // namespace behavior_velocity_planner + +#endif // UTILIZATION__MARKER_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp new file mode 100644 index 0000000000000..0a41a8846860c --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/path_utilization.hpp @@ -0,0 +1,34 @@ +// Copyright 2020 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 UTILIZATION__PATH_UTILIZATION_HPP_ +#define UTILIZATION__PATH_UTILIZATION_HPP_ + +#include +#include + +#include + +namespace behavior_velocity_planner +{ +autoware_auto_planning_msgs::msg::Path interpolatePath( + const autoware_auto_planning_msgs::msg::Path & path, const double length, + const double interval = 1.0); +autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_auto_planning_msgs::msg::Path & path); +autoware_auto_planning_msgs::msg::Path filterStopPathPoint( + const autoware_auto_planning_msgs::msg::Path & path); +} // namespace behavior_velocity_planner + +#endif // UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/state_machine.hpp b/planning/behavior_velocity_planner/include/utilization/state_machine.hpp new file mode 100644 index 0000000000000..657fab46555d0 --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/state_machine.hpp @@ -0,0 +1,96 @@ +// Copyright 2021 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 UTILIZATION__STATE_MACHINE_HPP_ +#define UTILIZATION__STATE_MACHINE_HPP_ + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +/** + * @brief Manage stop-go states with safety margin time. + */ +class StateMachine +{ +public: + enum State { + STOP = 0, + GO, + }; + std::string toString(const State & state) + { + if (state == State::STOP) { + return "STOP"; + } else if (state == State::GO) { + return "GO"; + } else { + return ""; + } + } + StateMachine() + { + state_ = State::GO; + margin_time_ = 0.0; + duration_ = 0.0; + } + void setStateWithMarginTime(State state, rclcpp::Logger logger, rclcpp::Clock & clock) + { + /* same state request */ + if (state_ == state) { + start_time_ = nullptr; // reset timer + return; + } + + /* GO -> STOP */ + if (state == State::STOP) { + state_ = State::STOP; + start_time_ = nullptr; // reset timer + return; + } + + /* STOP -> GO */ + if (state == State::GO) { + if (start_time_ == nullptr) { + start_time_ = std::make_shared(clock.now()); + } else { + duration_ = (clock.now() - *start_time_).seconds(); + if (duration_ > margin_time_) { + state_ = State::GO; + start_time_ = nullptr; // reset timer + } + } + return; + } + RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); + } + + void setMarginTime(const double t) { margin_time_ = t; } + void setState(State state) { state_ = state; } + State getState() const { return state_; } + double getDuration() const { return duration_; } + +private: + State state_; //! current state + double margin_time_; //! margin time when transit to Go from Stop + double duration_; //! duration time when transit to Go from Stop + std::shared_ptr start_time_; //! first time received GO when STOP state +}; + +} // namespace behavior_velocity_planner +#endif // UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp new file mode 100644 index 0000000000000..fdc1376d8ea85 --- /dev/null +++ b/planning/behavior_velocity_planner/include/utilization/util.hpp @@ -0,0 +1,155 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 UTILIZATION__UTIL_HPP_ +#define UTILIZATION__UTIL_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +using Point2d = boost::geometry::model::d2::point_xy; +namespace planning_utils +{ +inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Point & p) { return p; } +inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::Pose & p) { return p.position; } +inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseStamped & p) +{ + return p.pose.position; +} +inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p) +{ + return p.pose.position; +} +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose.position; +} +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +{ + return p.pose.position; +} +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::Path & path, int idx) +{ + return path.points.at(idx).pose; +} +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, int idx) +{ + return path.points.at(idx).point.pose; +} +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::Trajectory & traj, int idx) +{ + return traj.points.at(idx).pose; +} + +inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } + +inline double square(const double & a) { return a * a; } +double normalizeEulerAngle(double euler); +geometry_msgs::msg::Quaternion getQuaternionFromYaw(double yaw); + +template +double calcSquaredDist2d(const T1 & a, const T2 & b) +{ + return square(getPoint(a).x - getPoint(b).x) + square(getPoint(a).y - getPoint(b).y); +} + +template +double calcDist2d(const T1 & a, const T2 & b) +{ + return std::sqrt(calcSquaredDist2d(a, b)); +} + +template +bool calcClosestIndex( + const T & path, const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr = 3.0, + double angle_thr = M_PI_4); + +template +bool calcClosestIndex( + const T & path, const geometry_msgs::msg::Point & point, int & closest, double dist_thr = 3.0); + +geometry_msgs::msg::Pose transformRelCoordinate2D( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); +geometry_msgs::msg::Pose transformAbsCoordinate2D( + const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin); +Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object); +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); +Polygon2d generatePathPolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const size_t end_idx, const double width); + +double calcJudgeLineDistWithAccLimit( + const double velocity, const double max_stop_acceleration, const double delay_response_time); + +double calcJudgeLineDistWithJerkLimit( + const double velocity, const double acceleration, const double max_stop_acceleration, + const double max_stop_jerk, const double delay_response_time); + +autoware_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason); + +void appendStopReason( + const autoware_planning_msgs::msg::StopFactor stop_factor, + autoware_planning_msgs::msg::StopReason * stop_reason); + +std::vector toRosPoints( + const autoware_auto_perception_msgs::msg::PredictedObjects & object); + +geometry_msgs::msg::Point toRosPoint(const pcl::PointXYZ & pcl_point); +geometry_msgs::msg::Point toRosPoint(const Point2d & boost_point, const double z); + +LineString2d extendLine( + const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2, + const double & length); + +template +std::vector concatVector(const std::vector & vec1, const std::vector & vec2) +{ + auto concat_vec = vec1; + concat_vec.insert(std::end(concat_vec), std::begin(vec2), std::end(vec2)); + return concat_vec; +} +} // namespace planning_utils +} // namespace behavior_velocity_planner + +#endif // UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner/intersection-design.md b/planning/behavior_velocity_planner/intersection-design.md new file mode 100644 index 0000000000000..4e12cf401bbbc --- /dev/null +++ b/planning/behavior_velocity_planner/intersection-design.md @@ -0,0 +1,182 @@ +### Intersection + +#### Role + +Judgement whether a vehicle can go into an intersection or not by a dynamic object information, and planning a velocity of the low-down/stop. +This module is designed for rule-based intersection velocity decision that is easy for developers to design its behavior. It generates proper velocity for intersection scene. + +In addition, the external users / modules (e.g. remote operation) to can intervene the STOP/GO decision for the vehicle behavior. The override interface is expected to be used, for example, for remote intervention in emergency situations or gathering information on operator decisions during development. + +![brief](./docs/intersection/intersection.svg) + +### Limitations + +This module allows developers to design vehicle velocity in intersection module using specific rules. This module is affected by object detection and prediction accuracy considering as stuck vehicle in this intersection module. + +### Inner-workings / Algorithms + +#### Attention Target Objects + +Car, truck, bus, motorbike are included in the attention target object for the velocity planning in the intersection and bicycle, pedestrian, animal, unknown are not. + +#### Attention Lane + +The lane crossing with driving lane and has high priority for the driving lane is defined as attention lane. (Priority tags are needed to be configured according to the situation.) + +#### Crossing Judgement + +Time to pass the intersection will be calculated with the length of the intersection (Supposed constant velocity : 10km/h). Driving lane polygon is defined from the position of the ego vehicle to the end point of the lane with intersection tag. It is checked if the polygon and the predicted path of the attention target objects will be crossed. + +#### State Transition (go / stop) + +If there is no crossing more than a certain period (default : 2.0s), the state transits to “go”. If crossing is detected even once, the state transits to “stop”. + +#### Stop Line Automatic Generation + +The driving lane is complemented at a certain intervals (default : 20cm), and the line which is a margin distance (default : 100cm) in front of the attention lane is defined as a stop line. (Also the length of the vehicle is considered and the stop point is set at the base_link point in front of the stop lane.) + +#### Pass Judge Line + +To avoid a rapid braking, in case that a deceleration more than a threshold (default : 0.5G) is needed, the ego vehicle doesn’t stop. In order to judge this condition, pass judge line is set a certain distance (default : 0.5\*v_curr^2/a_max) in front of the stop line. +To prevent a chattering, once the ego vehicle passes this line, “stop” decision in the intersection won’t be done any more. +To prevent going over the pass judge line before the traffic light stop line, the distance between stopline and pass judge line become 0m in case that there is a stop line between the ego vehicle and an intersection stop line. + +#### Vehicle In a Same Lane Removal + +Ignore the object in front of/ behind the ego vehicle in the same lane (Improvement needed : the linkage with the uuid in tracking is needed) + +#### Stuck vehicle + +If there is any object in a certain distance (default : 5m) from the end point of the intersection lane on the driving lane and the object velocity is less than a threshold (default 3.0km/h), the object is regarded as a stuck vehicle. If the stuck vehicle exists, the ego vehicle cannot enter the intersection. + +### Launch Timing + +Launches when there is a conflicting lanelet in ego lane. + +### How to Decide Intersection Stop + +The intersection stop target should be limited to stuck vehicle in the middle of the road or incoming cruising vehicle that will collide with ego vehicle. Therefore, target vehicles for considering intersection stop meet the following specific conditions. + +- It is inside detection area polygon created in this module and is stopped(parametrized) + - This means that moving object inside ego lane is not considered as stop target. +- It is a specific class. + - User can change intersection stop targets (e.g. do not stop unknown-class targets). +- It will collide with ego vehicle. + - This means that the other incoming vehicle from conflicting lanelet can collide with ego vehicle. + +### Module Parameters + +| Parameter | Type | Description | +| --------------------------------------------- | ------ | ----------------------------------------------------------------------------- | +| `intersection/state_transit_margin_time` | double | [m] time margin to change state | +| `intersection/decel_velocity` | double | [m] deceleration velocity in intersection | +| `intersection/path_expand_width` | bool | [m] path area to see with expansion | +| `intersection/stop_line_margin` | double | [m] margin before stop line | +| `intersection/stuck_vehicle_detect_dist` | double | [m] this should be the length between cars when they are stopped. | +| `intersection/stuck_vehicle_ignore_dist` | double | [m] obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) | +| `intersection/stuck_vehicle_vel_thr` | double | [m/s] velocity below 3[km/h] is ignored by default | +| `intersection/intersection_velocity` | double | [m/s] velocity to pass intersection. 10[km/h] is by default | +| `intersection/intersection_max_accel` | double | [m/s^2] acceleration in intersection | +| `intersection/detection_area_margin` | double | [m] range for expanding detection area | +| `intersection/detection_area_length` | double | [m] range for lidar detection 200m is by default | +| `intersection/detection_area_angle_threshold` | double | [rad] threshold of angle difference between the detection object and lane | +| `intersection/min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | +| `merge_from_private_road/stop_duration_sec` | double | [s] duration to stop | + +### How To Tune Parameters + +- The time to change state form `Stop` to `GO` is too long. + - Change `state_transit_margin_time` to lower value. Be careful if this margin is too small then vehicle is going to change state many times and cause chattering. +- The distance to stuck vehicle is too long. + - Change `stuck_vehicle_detect_dist` to lower value. Note this module consider obstacle stop max distance as detection distance. +- The speed in intersection is too slow +- Change `intersection_velocity` to higher value. + +### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +:get object polygon; + +partition get_objective_polygon { + +:get "right-of-way" lanelets from ego lanelet; + +:get previous and following ego lanelet from routing graph; + +:get all conflicting lanelets with ego lane; + +:exclude yield and ego lanelets from conflicting lanelets; + +:update conflicting and objective polygon; +} + +:get external Input; + +:generate stop line; + +if (not generate stop line) then (yes) + stop +else (no) +endif + +if (stop line index or pass judge index is same lane as ego?) then (yes) + stop +else (no) +endif + +if (not found closest index?) then (yes) + stop +else (no) +endif + +if (state is GO and over pass judge line and no external stop) then (yes) + stop +else (no) +endif + +if (has collision or is stuck vehicle in intersection?) then (yes) + :set is_entry_prohibited true; + :set state from external input; +else (no) +endif + +:set state with margin time; + +if (current state is same as previous state) then (yes) + :reset timer; +else if (state is GO->STOP) then (yes) + :set state as STOP; + :reset timer; +else if (state is STOP -> GO) then (yes) + if (start time is not set) then (yes) + :set start time; + else(no) + :calculate duration; + if(duration is more than margin time)then (yes) + :set state GO; + :reset timer; + endif + endif +endif + +if (state is STOP?) then (yes) + :set velocity; + if (stop required?) then (yes) + :publish stop reason and stop factor; + endif +else(no) +endif +stop + +@enduml +``` + +NOTE current state is treated as `STOP` if `is_entry_prohibited` = `true` else `GO` + +### Known Limits + +- This module generate intersection stop line and ignoring lanelet automatically form lanelet map , however if you want to set intersection stop line and ignoring lanelet manually you need to tag `right_of_way` and `yield` to all conflicting lanes properly. diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml new file mode 100644 index 0000000000000..07d592f3695ab --- /dev/null +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -0,0 +1,69 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_planner/no-stopping-area-design.md b/planning/behavior_velocity_planner/no-stopping-area-design.md new file mode 100644 index 0000000000000..4672f2d162d1c --- /dev/null +++ b/planning/behavior_velocity_planner/no-stopping-area-design.md @@ -0,0 +1,125 @@ +### No Stopping Area + +#### Role + +This module plans to avoid stop in 'no stopping area`. + +![brief](./docs/no_stopping_area/NoStoppingArea.svg) + +- PassThrough case + - if ego vehicle go through pass judge point, then ego vehicle can't stop with maximum jerk and acceleration, so this module doesn't insert stop velocity. In this case override or external operation is necessary. +- STOP case + - If there is a stuck vehicle or stop velocity around `no_stopping_area`, then vehicle stops inside `no_stopping_area` so this module makes stop velocity in front of `no_stopping_area` +- GO case + - else + +### Limitation + +This module allows developers to design vehicle velocity in `no_stopping_area` module using specific rules. Once ego vehicle go through pass through point, ego vehicle does't insert stop velocity and does't change decision from GO. + +#### ModelParameter + +| Parameter | Type | Description | +| ---------------------------- | ------ | ------------------------------------------------------------------- | +| `state_clear_time` | double | [s] time to clear stop state | +| `stuck_vehicle_vel_thr` | double | [m/s] vehicles below this velocity are considered as stuck vehicle. | +| `stop_margin` | double | [m] margin to stop line at no stopping area | +| `dead_line_margin` | double | [m] if ego pass this position GO | +| `stop_line_margin` | double | [m] margin to auto-gen stop line at no stopping area | +| `detection_area_length` | double | [m] length of searching polygon | +| `stuck_vehicle_front_margin` | double | [m] obstacle stop max distance | + +#### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +if (ego path has "no stopping area" ?) then (yes) +else (no) + stop +endif + +partition pass_through_condition { +if (ego vehicle is not after dead line?) then (yes) +else (no) + stop +endif +if (ego vehicle is stoppable before stop line consider jerk limit?) then (yes) +else (no) + stop +endif +} +note right + - ego vehicle is already over dead line(1.0[m] forward stop line) Do Not Stop. + - "pass through or not" considering jerk limit is judged only once to avoid chattering. +end note + +:generate ego "stuck_vehicle_detect_area" polygon; +note right +"stuck_vehicle_detect_area" polygon includes space of + vehicle_length + obstacle_stop_max_distance + after "no stopping area" +end note + +:generate ego "stop_line_detect_area" polygon; +note right +"stop_line_detect_area" polygon includes space of + vehicle_length + margin + after "no stopping area" +end note + +:set current judgement as GO; +if (Is stuck vehicle inside "stuck_vehicle_detect_area" polygon?) then (yes) +note right +only consider stuck vehicle following condition. +- below velocity 3.0 [m/s] +- semantic type of car bus truck or motorbike +only consider stop line as following condition. +- low velocity that is in path with lane id is considered. +end note +if (Is stop line inside "stop_line_detect_area" polygon?) then (yes) + :set current judgement as STOP; +endif +endif + +partition set_state_with_margin_time { + +if (current judgement is same as previous state) then (yes) + :reset timer; +else if (state is GO->STOP) then (yes) + :set state as STOP; + :reset timer; +else if (state is STOP -> GO) then (yes) + if (start time is not set) then (yes) + :set start time; + else(no) + :calculate duration; + if(duration is more than margin time)then (yes) + :set state GO; + :reset timer; + else(no) + endif + endif +else(no) +endif + +} + +note right + - it takes 2 seconds to change state from STOP -> GO + - it takes 0 seconds to change state from GO -> STOP + - reset timer if no state change +end note + +if (state is STOP) then (yes) + :set stop velocity; + :set stop reason and factor; + else(no) +endif +stop + + +@enduml +``` diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md new file mode 100644 index 0000000000000..187840f97bbab --- /dev/null +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -0,0 +1,90 @@ +### Occlusion Spot + +#### Role + +This module plans safe velocity to slow down before reaching collision point that hidden object is darting out from `occlusion spot` where driver can't see clearly because of obstacles. + +![brief](./docs/occlusion_spot/occlusion_spot.svg) + +#### Occlusion Spot Private + +This module only works in private road and use occupancy grid map to detect occlusion spots. + +#### Occlusion Spot Public + +This module only works in public road and use dynamic objects to detect occlusion spots. + +Considering all occupancy grid cells inside focus range requires a lot of computation cost, so this module ignores to search farther occlusion spot which is longitudinally or laterally slice once occlusion spot is found. + +![brief](./docs/occlusion_spot/sidewalk_slice.svg) + +##### Definition + +This module insert safe velocity at collision point and show virtual wall at intersection below. + +![brief](./docs/occlusion_spot/possible_collision_info.svg) + +#### Module Parameters + +| Parameter | Type | Description | +| -------------------- | ------ | ------------------------------------------------------------------------- | +| `pedestrian_vel` | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point | +| `safety_time_buffer` | double | [m/s] time buffer for the system delay | + +| Parameter /threshold | Type | Description | +| ----------------------- | ------ | --------------------------------------------------------- | +| `detection_area_length` | double | [m] the length of path to consider occlusion spot | +| `stuck_vehicle_vel` | double | [m/s] velocity below this value is assumed to stop | +| `lateral_distance` | double | [m] maximum lateral distance to consider hidden collision | + +| Parameter /(public or private)\_road | Type | Description | +| ------------------------------------ | ------ | -------------------------------------------------------------------- | +| `min_velocity` | double | [m/s] minimum velocity to ignore occlusion spot | +| `ebs_decel` | double | [m/s^2] maximum deceleration to assume for emergency braking system. | +| `pbs_decel` | double | [m/s^2] deceleration to assume for predictive braking system | + +| Parameter /sidewalk | Type | Description | +| ------------------------- | ------ | --------------------------------------------------------------- | +| `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot | +| `focus_range` | double | [m] buffer around the ego path used to build the sidewalk area. | + +| Parameter /grid | Type | Description | +| ---------------- | ------ | --------------------------------------------------------------- | +| `free_space_max` | double | [-] maximum value of a free space cell in the occupancy grid | +| `occupied_min` | double | [-] buffer around the ego path used to build the sidewalk area. | + +#### Flowchart + +```plantuml +@startuml +title modifyPathVelocity (Private/Public) Road +start + +:get current road type; + +if (road type is PUBLIC) then (yes) + :use dynamic object array info; +else if (road type is PRIVATE) then (yes) + :use occupancy grid map info; +else (no) + stop +endif + +:generate possible collision; + +:find possible collision between path and occlusion spot; + +if (possible collision is found?) then (yes) +else (no) + stop +endif + +:calculate collision path point; + +:calculate safe velocity consider lateral distance and safe velocity; + +:insertSafeVelocityToPath; + +stop +@enduml +``` diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml new file mode 100644 index 0000000000000..e88c614a7aa7a --- /dev/null +++ b/planning/behavior_velocity_planner/package.xml @@ -0,0 +1,49 @@ + + + + behavior_velocity_planner + 0.1.0 + The behavior_velocity_planner package + + Yukihiro Saito + Apache License 2.0 + + ament_cmake + eigen3_cmake_module + + autoware_api_msgs + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_planning_msgs + autoware_utils + autoware_v2x_msgs + diagnostic_msgs + eigen + geometry_msgs + grid_map_ros + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + nav_msgs + nlohmann-json-dev + pcl_conversions + rclcpp + rclcpp_components + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + vehicle_info_util + visualization_msgs + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp new file mode 100644 index 0000000000000..65f25f2c176bc --- /dev/null +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -0,0 +1,389 @@ +// Copyright 2020 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 "behavior_velocity_planner/node.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +// Scene modules +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +geometry_msgs::msg::PoseStamped transform2pose( + const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header = transform.header; + pose.pose.position.x = transform.transform.translation.x; + pose.pose.position.y = transform.transform.translation.y; + pose.pose.position.z = transform.transform.translation.z; + pose.pose.orientation = transform.transform.rotation; + return pose; +} + +autoware_auto_planning_msgs::msg::Path to_path( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) +{ + autoware_auto_planning_msgs::msg::Path path; + for (const auto & path_point : path_with_id.points) { + path.points.push_back(path_point.point); + } + return path; +} +} // namespace + +BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptions & node_options) +: Node("behavior_velocity_planner_node", node_options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + planner_data_(*this) +{ + using std::placeholders::_1; + // Trigger Subscriber + trigger_sub_path_with_lane_id_ = + this->create_subscription( + "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); + + // Subscribers + sub_predicted_objects_ = + this->create_subscription( + "~/input/dynamic_objects", 1, + std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1)); + sub_no_ground_pointcloud_ = this->create_subscription( + "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), + std::bind(&BehaviorVelocityPlannerNode::onNoGroundPointCloud, this, _1)); + sub_vehicle_odometry_ = this->create_subscription( + "~/input/vehicle_odometry", 1, + std::bind(&BehaviorVelocityPlannerNode::onVehicleVelocity, this, _1)); + sub_lanelet_map_ = this->create_subscription( + "~/input/vector_map", rclcpp::QoS(10).transient_local(), + std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1)); + sub_traffic_signals_ = + this->create_subscription( + "~/input/traffic_signals", 10, + std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1)); + sub_external_crosswalk_states_ = + this->create_subscription( + "~/input/external_crosswalk_states", 10, + std::bind(&BehaviorVelocityPlannerNode::onExternalCrosswalkStates, this, _1)); + sub_external_intersection_states_ = + this->create_subscription( + "~/input/external_intersection_states", 10, + std::bind(&BehaviorVelocityPlannerNode::onExternalIntersectionStates, this, _1)); + sub_external_traffic_signals_ = + this->create_subscription( + "~/input/external_traffic_signals", 10, + std::bind(&BehaviorVelocityPlannerNode::onExternalTrafficSignals, this, _1)); + sub_virtual_traffic_light_states_ = + this->create_subscription( + "~/input/virtual_traffic_light_states", 10, + std::bind(&BehaviorVelocityPlannerNode::onVirtualTrafficLightStates, this, _1)); + sub_occupancy_grid_ = this->create_subscription( + "~/input/occupancy_grid", 1, + std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1)); + + // Publishers + path_pub_ = this->create_publisher("~/output/path", 1); + stop_reason_diag_pub_ = + this->create_publisher("~/output/stop_reason", 1); + debug_viz_pub_ = this->create_publisher("~/debug/path", 1); + + // Parameters + forward_path_length_ = this->declare_parameter("forward_path_length", 1000.0); + backward_path_length_ = this->declare_parameter("backward_path_length", 5.0); + // TODO(yukkysaito): This will become unnecessary when acc output from localization is available. + planner_data_.accel_lowpass_gain_ = this->declare_parameter("lowpass_gain", 0.5); + planner_data_.stop_line_extend_length = this->declare_parameter("stop_line_extend_length", 5.0); + + // Initialize PlannerManager + if (this->declare_parameter("launch_crosswalk", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + if (this->declare_parameter("launch_traffic_light", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + if (this->declare_parameter("launch_intersection", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + if (this->declare_parameter("launch_blind_spot", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + if (this->declare_parameter("launch_detection_area", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + if (this->declare_parameter("launch_virtual_traffic_light", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + if (this->declare_parameter("launch_occlusion_spot", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + // this module requires all the stop line.Therefore this modules should be placed at the bottom. + if (this->declare_parameter("launch_no_stopping_area", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } + // permanent stop line module should be after no stopping area + if (this->declare_parameter("launch_stop_line", true)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } +} + +bool BehaviorVelocityPlannerNode::isDataReady() +{ + const auto & d = planner_data_; + + // from tf + if (d.current_pose.header.frame_id == "") { + return false; + } + + // from callbacks + if (!d.current_velocity) { + return false; + } + if (!d.predicted_objects) { + return false; + } + if (!d.no_ground_pointcloud) { + return false; + } + if (!d.lanelet_map) { + return false; + } + + return true; +} + +void BehaviorVelocityPlannerNode::onOccupancyGrid( + const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) +{ + planner_data_.occupancy_grid = msg; +} + +void BehaviorVelocityPlannerNode::onPredictedObjects( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) +{ + planner_data_.predicted_objects = msg; +} + +void BehaviorVelocityPlannerNode::onNoGroundPointCloud( + 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(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); + + planner_data_.no_ground_pointcloud = pc_transformed; +} + +void BehaviorVelocityPlannerNode::onVehicleVelocity( + const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + auto current_velocity = std::make_shared(); + current_velocity->header = msg->header; + current_velocity->twist = msg->twist.twist; + planner_data_.current_velocity = current_velocity; + + planner_data_.updateCurrentAcc(); + + // Add velocity to buffer + planner_data_.velocity_buffer.push_front(*current_velocity); + const auto now = this->now(); + while (true) { + // Check oldest data time + const auto time_diff = now - planner_data_.velocity_buffer.back().header.stamp; + + // Finish when oldest data is newer than threshold + if (time_diff.seconds() <= PlannerData::velocity_buffer_time_sec) { + break; + } + + // Remove old data + planner_data_.velocity_buffer.pop_back(); + } +} + +void BehaviorVelocityPlannerNode::onLaneletMap( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +{ + // Load map + planner_data_.lanelet_map = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *msg, planner_data_.lanelet_map, &planner_data_.traffic_rules, &planner_data_.routing_graph); + + // Build graph + { + using lanelet::Locations; + using lanelet::Participants; + using lanelet::routing::RoutingGraph; + using lanelet::routing::RoutingGraphConstPtr; + using lanelet::routing::RoutingGraphContainer; + using lanelet::traffic_rules::TrafficRulesFactory; + + const auto traffic_rules = + TrafficRulesFactory::create(Locations::Germany, Participants::Vehicle); + const auto pedestrian_rules = + TrafficRulesFactory::create(Locations::Germany, Participants::Pedestrian); + + RoutingGraphConstPtr vehicle_graph = + RoutingGraph::build(*planner_data_.lanelet_map, *traffic_rules); + RoutingGraphConstPtr pedestrian_graph = + RoutingGraph::build(*planner_data_.lanelet_map, *pedestrian_rules); + RoutingGraphContainer overall_graphs({vehicle_graph, pedestrian_graph}); + + planner_data_.overall_graphs = std::make_shared(overall_graphs); + } +} + +void BehaviorVelocityPlannerNode::onTrafficSignals( + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + for (const auto & signal : msg->signals) { + autoware_auto_perception_msgs::msg::TrafficSignalStamped traffic_signal; + traffic_signal.header = msg->header; + traffic_signal.signal = signal; + planner_data_.traffic_light_id_map[signal.map_primitive_id] = traffic_signal; + } +} + +void BehaviorVelocityPlannerNode::onExternalCrosswalkStates( + const autoware_api_msgs::msg::CrosswalkStatus::ConstSharedPtr msg) +{ + planner_data_.external_crosswalk_status_input = *msg; +} + +void BehaviorVelocityPlannerNode::onExternalIntersectionStates( + const autoware_api_msgs::msg::IntersectionStatus::ConstSharedPtr msg) +{ + planner_data_.external_intersection_status_input = *msg; +} + +void BehaviorVelocityPlannerNode::onExternalTrafficSignals( + const autoware_auto_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) +{ + for (const auto & signal : msg->signals) { + autoware_auto_perception_msgs::msg::TrafficSignalStamped traffic_signal; + traffic_signal.header = msg->header; + traffic_signal.signal = signal; + planner_data_.external_traffic_light_id_map[signal.map_primitive_id] = traffic_signal; + } +} + +void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( + const autoware_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) +{ + planner_data_.virtual_traffic_light_states = msg; +} + +void BehaviorVelocityPlannerNode::onTrigger( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) +{ + // Check ready + try { + planner_data_.current_pose = + transform2pose(tf_buffer_.lookupTransform("map", "base_link", tf2::TimePointZero)); + } catch (tf2::TransformException & e) { + RCLCPP_INFO(get_logger(), "waiting for transform from `map` to `base_link`"); + return; + } + + if (!isDataReady()) { + return; + } + + // Plan path velocity + const auto velocity_planned_path = planner_manager_.planPathVelocity( + std::make_shared(planner_data_), *input_path_msg); + + // screening + const auto filtered_path = filterLitterPathPoint(to_path(velocity_planned_path)); + + // interpolation + const auto interpolated_path_msg = interpolatePath(filtered_path, forward_path_length_); + + // check stop point + auto output_path_msg = filterStopPathPoint(interpolated_path_msg); + output_path_msg.header.frame_id = "map"; + output_path_msg.header.stamp = this->now(); + + // TODO(someone): This must be updated in each scene module, but copy from input message for now. + output_path_msg.drivable_area = input_path_msg->drivable_area; + + path_pub_->publish(output_path_msg); + stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); + + if (debug_viz_pub_->get_subscription_count() > 0) { + publishDebugMarker(output_path_msg); + } +} + +void BehaviorVelocityPlannerNode::publishDebugMarker( + const autoware_auto_planning_msgs::msg::Path & path) +{ + visualization_msgs::msg::MarkerArray output_msg; + for (size_t i = 0; i < path.points.size(); ++i) { + visualization_msgs::msg::Marker marker; + marker.header = path.header; + marker.id = i; + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.pose = path.points.at(i).pose; + marker.scale.y = marker.scale.z = 0.05; + marker.scale.x = 0.25; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + output_msg.markers.push_back(marker); + } + debug_viz_pub_->publish(output_msg); +} +} // namespace behavior_velocity_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(behavior_velocity_planner::BehaviorVelocityPlannerNode) diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp new file mode 100644 index 0000000000000..44ce376a0983c --- /dev/null +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -0,0 +1,90 @@ +// Copyright 2020 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 "behavior_velocity_planner/planner_manager.hpp" + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::string jsonDumpsPose(const geometry_msgs::msg::Pose & pose) +{ + const std::string json_dumps_pose = + (boost::format( + R"({"position":{"x":%lf,"y":%lf,"z":%lf},"orientation":{"w":%lf,"x":%lf,"y":%lf,"z":%lf}})") % + pose.position.x % pose.position.y % pose.position.z % pose.orientation.w % pose.orientation.x % + pose.orientation.y % pose.orientation.z) + .str(); + return json_dumps_pose; +} + +diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( + const std::string stop_reason, const geometry_msgs::msg::Pose & stop_pose) +{ + diagnostic_msgs::msg::DiagnosticStatus stop_reason_diag; + diagnostic_msgs::msg::KeyValue stop_reason_diag_kv; + stop_reason_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stop_reason_diag.name = "stop_reason"; + stop_reason_diag.message = stop_reason; + stop_reason_diag_kv.key = "stop_pose"; + stop_reason_diag_kv.value = jsonDumpsPose(stop_pose); + stop_reason_diag.values.push_back(stop_reason_diag_kv); + return stop_reason_diag; +} +} // namespace + +void BehaviorVelocityPlannerManager::launchSceneModule( + const std::shared_ptr & scene_module_manager_ptr) +{ + scene_manager_ptrs_.push_back(scene_module_manager_ptr); +} + +autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; + + int first_stop_path_point_index = static_cast(output_path_msg.points.size() - 1); + std::string stop_reason_msg("path_end"); + + for (const auto & scene_manager_ptr : scene_manager_ptrs_) { + scene_manager_ptr->updateSceneModuleInstances(planner_data, input_path_msg); + scene_manager_ptr->modifyPathVelocity(&output_path_msg); + boost::optional firstStopPathPointIndex = scene_manager_ptr->getFirstStopPathPointIndex(); + + if (firstStopPathPointIndex) { + if (firstStopPathPointIndex.get() < first_stop_path_point_index) { + first_stop_path_point_index = firstStopPathPointIndex.get(); + stop_reason_msg = scene_manager_ptr->getModuleName(); + } + } + } + + stop_reason_diag_ = makeStopReasonDiag( + stop_reason_msg, output_path_msg.points[first_stop_path_point_index].point.pose); + + return output_path_msg; +} + +diagnostic_msgs::msg::DiagnosticStatus BehaviorVelocityPlannerManager::getStopReasonDiag() +{ + return stop_reason_diag_; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp new file mode 100644 index 0000000000000..328c3eefe9589 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/debug.cpp @@ -0,0 +1,238 @@ +// Copyright 2020 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 +#include +#include + +#include + +namespace behavior_velocity_planner +{ +namespace +{ +using State = BlindSpotModule::State; + +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const lanelet::CompoundPolygon3d & polygon, const std::string & ns, const int64_t lane_id, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + int32_t uid = planning_utils::bitShift(lane_id); + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + + marker.ns = ns; + marker.id = uid; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = createMarkerScale(0.3, 0.0, 0.0); + marker.color = createMarkerColor(r, g, b, 0.8); + for (const auto & p : polygon) { + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + marker.points.push_back(point); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t lane_id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = ns; + + int32_t uid = planning_utils::bitShift(lane_id); + int32_t i = 0; + for (const auto & object : objects.objects) { + marker.id = uid + i++; + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = object.kinematics.initial_pose_with_covariance.pose; + marker.scale = createMarkerScale(1.0, 1.0, 1.0); + marker.color = createMarkerColor(r, g, b, 0.8); + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = ns; + marker.id = lane_id; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = createMarkerScale(0.3, 0.0, 0.0); + marker.color = createMarkerColor(r, g, b, 0.999); + + for (const auto & p : path.points) { + marker.points.push_back(p.point.pose.position); + } + + msg.markers.push_back(marker); + + return msg; +} + +visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( + const geometry_msgs::msg::Pose & pose, const int64_t lane_id) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker_virtual_wall{}; + marker_virtual_wall.header.frame_id = "map"; + marker_virtual_wall.ns = "stop_virtual_wall"; + marker_virtual_wall.id = lane_id; + marker_virtual_wall.lifetime = rclcpp::Duration::from_seconds(0.5); + marker_virtual_wall.type = visualization_msgs::msg::Marker::CUBE; + marker_virtual_wall.action = visualization_msgs::msg::Marker::ADD; + marker_virtual_wall.pose = pose; + marker_virtual_wall.pose.position.z += 1.0; + marker_virtual_wall.scale = createMarkerScale(0.1, 5.0, 2.0); + marker_virtual_wall.color = createMarkerColor(1.0, 0.0, 0.0, 0.5); + msg.markers.push_back(marker_virtual_wall); + + visualization_msgs::msg::Marker marker_factor_text{}; + marker_factor_text.header.frame_id = "map"; + marker_factor_text.ns = "factor_text"; + marker_factor_text.id = lane_id; + marker_factor_text.lifetime = rclcpp::Duration::from_seconds(0.5); + marker_factor_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_factor_text.action = visualization_msgs::msg::Marker::ADD; + marker_factor_text.pose = pose; + marker_factor_text.pose.position.z += 2.0; + marker_factor_text.scale = createMarkerScale(0.0, 0.0, 1.0); + marker_factor_text.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); + marker_factor_text.text = "blind spot"; + msg.markers.push_back(marker_factor_text); + + return msg; +} + +visualization_msgs::msg::MarkerArray createPoseMarkerArray( + const geometry_msgs::msg::Pose & pose, const State & state, const std::string & ns, + const int64_t id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + if (state == State::STOP) { + visualization_msgs::msg::Marker marker_line{}; + marker_line.header.frame_id = "map"; + marker_line.ns = ns + "_line"; + marker_line.id = id; + marker_line.lifetime = rclcpp::Duration::from_seconds(0.3); + marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_line.action = visualization_msgs::msg::Marker::ADD; + marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); + marker_line.color = createMarkerColor(r, g, b, 0.999); + + const double yaw = tf2::getYaw(pose.orientation); + + const double a = 3.0; + geometry_msgs::msg::Point p0; + p0.x = pose.position.x - a * std::sin(yaw); + p0.y = pose.position.y + a * std::cos(yaw); + p0.z = pose.position.z; + marker_line.points.push_back(p0); + + geometry_msgs::msg::Point p1; + p1.x = pose.position.x + a * std::sin(yaw); + p1.y = pose.position.y - a * std::cos(yaw); + p1.z = pose.position.z; + marker_line.points.push_back(p1); + + msg.markers.push_back(marker_line); + } + + return msg; +} + +} // namespace + +visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + const auto state = state_machine_.getState(); + const auto current_time = this->clock_->now(); + + appendMarkerArray( + createPathMarkerArray(debug_data_.path_raw, "path_raw", lane_id_, 0.0, 1.0, 1.0), current_time, + &debug_marker_array); + + appendMarkerArray( + createPoseMarkerArray( + debug_data_.stop_point_pose, state, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), + current_time, &debug_marker_array); + + appendMarkerArray( + createPoseMarkerArray( + debug_data_.judge_point_pose, state, "judge_point_pose", lane_id_, 1.0, 1.0, 0.5), + current_time, &debug_marker_array); + + appendMarkerArray( + createPolygonMarkerArray( + debug_data_.conflict_area_for_blind_spot, "conflict_area_for_blind_spot", lane_id_, 0.0, 0.5, + 0.5), + current_time, &debug_marker_array); + + appendMarkerArray( + createPolygonMarkerArray( + debug_data_.detection_area_for_blind_spot, "detection_area_for_blind_spot", lane_id_, 0.0, + 0.5, 0.5), + current_time, &debug_marker_array); + + appendMarkerArray( + createObjectsMarkerArray( + debug_data_.conflicting_targets, "conflicting_targets", lane_id_, 0.99, 0.4, 0.0), + current_time, &debug_marker_array); + + appendMarkerArray( + createPathMarkerArray(debug_data_.spline_path, "spline", lane_id_, 0.5, 0.5, 0.5), current_time, + &debug_marker_array); + + if (state == BlindSpotModule::State::STOP) { + appendMarkerArray( + createVirtualWallMarkerArray(debug_data_.virtual_wall_pose, lane_id_), current_time, + &debug_marker_array); + } + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp new file mode 100644 index 0000000000000..f4edbc1b737a2 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp @@ -0,0 +1,103 @@ +// Copyright 2020 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 +#include +#include + +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::vector getLaneletsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector lanelets; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); + } + + return lanelets; +} + +std::set getLaneIdSetOnPath(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + std::set lane_id_set; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + lane_id_set.insert(lane_id); + } + + return lane_id_set; +} + +} // namespace + +BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + planner_param_.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + planner_param_.backward_length = node.declare_parameter(ns + ".backward_length", 15.0); + planner_param_.ignore_width_from_center_line = + node.declare_parameter(ns + ".ignore_width_from_center_line", 1.0); + planner_param_.max_future_movement_time = + node.declare_parameter(ns + ".max_future_movement_time", 10.0); +} + +void BlindSpotModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & ll : getLaneletsOnPath(path, planner_data_->lanelet_map)) { + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + + // Is turning lane? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction != "left" && turn_direction != "right") { + continue; + } + + registerModule(std::make_shared( + module_id, lane_id, planner_data_, planner_param_, logger_.get_child("blind_spot_module"), + clock_)); + } +} + +std::function &)> +BlindSpotModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_id_set = getLaneIdSetOnPath(path); + + return [lane_id_set](const std::shared_ptr & scene_module) { + return lane_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp new file mode 100644 index 0000000000000..cda05992ec906 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -0,0 +1,593 @@ +// Copyright 2020 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +BlindSpotModule::BlindSpotModule( + const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + lane_id_(lane_id), + turn_direction_(TurnDirection::INVALID) +{ + planner_param_ = planner_param; + const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); + if (!turn_direction.compare("left")) { + turn_direction_ = TurnDirection::LEFT; + } else if (!turn_direction.compare("right")) { + turn_direction_ = TurnDirection::RIGHT; + } + has_traffic_light_ = + !(assigned_lanelet.regulatoryElementsAs().empty()); +} + +bool BlindSpotModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::BLIND_SPOT); + + const auto input_path = *path; + debug_data_.path_raw = input_path; + + State current_state = state_machine_.getState(); + RCLCPP_DEBUG(logger_, "lane_id = %ld, state = %s", lane_id_, toString(current_state).c_str()); + + /* get current pose */ + geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; + + /* get lanelet map */ + const auto lanelet_map_ptr = planner_data_->lanelet_map; + const auto routing_graph_ptr = planner_data_->routing_graph; + + /* set stop-line and stop-judgement-line for base_link */ + int stop_line_idx = -1; + int pass_judge_line_idx = -1; + const auto straight_lanelets = getStraightLanelets(lanelet_map_ptr, routing_graph_ptr, lane_id_); + if (!generateStopLine(straight_lanelets, path, &stop_line_idx, &pass_judge_line_idx)) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, *clock_, 1000 /* ms */, "[BlindSpotModule::run] setStopLineIdx fail"); + *path = input_path; // reset path + return false; + } + + if (stop_line_idx <= 0 || pass_judge_line_idx <= 0) { + RCLCPP_DEBUG( + logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); + *path = input_path; // reset path + return true; + } + + /* calc closest index */ + int closest_idx = -1; + if (!planning_utils::calcClosestIndex(input_path, current_pose.pose, closest_idx)) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, *clock_, 1000 /* ms */, "[Blind Spot] calcClosestIndex fail"); + *path = input_path; // reset path + return false; + } + + /* get debug info */ + const auto stop_line_pose = util::getAheadPose( + stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + debug_data_.virtual_wall_pose = stop_line_pose; + debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; + debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.pose; + + /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ + bool is_over_pass_judge_line = static_cast(closest_idx > pass_judge_line_idx); + if (closest_idx == pass_judge_line_idx) { + geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose; + is_over_pass_judge_line = util::isAheadOf(current_pose.pose, pass_judge_line); + } + if (current_state == State::GO && is_over_pass_judge_line) { + RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); + *path = input_path; // reset path + return true; // no plan needed. + } + + /* get dynamic object */ + const auto objects_ptr = planner_data_->predicted_objects; + + /* calculate dynamic collision around detection area */ + bool has_obstacle = checkObstacleInBlindSpot( + lanelet_map_ptr, routing_graph_ptr, *path, objects_ptr, closest_idx, stop_line_pose); + state_machine_.setStateWithMarginTime( + has_obstacle ? State::STOP : State::GO, logger_.get_child("state_machine"), *clock_); + + /* set stop speed */ + if (state_machine_.getState() == State::STOP) { + constexpr double stop_vel = 0.0; + util::setVelocityFrom(stop_line_idx, stop_vel, path); + + /* get stop point and stop factor */ + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.stop_point_pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data_.conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + } else { + *path = input_path; // reset path + } + + return true; +} + +boost::optional BlindSpotModule::getFirstPointConflictingLanelets( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::ConstLanelets & lanelets) const +{ + using lanelet::utils::to2D; + using lanelet::utils::toHybrid; + + int first_idx_conflicting_lanelets = path.points.size() - 1; + bool is_conflict = false; + for (const auto & ll : lanelets) { + const auto line = (turn_direction_ == TurnDirection::LEFT) ? ll.leftBound() : ll.rightBound(); + for (size_t i = 0; i < path.points.size(); ++i) { + const auto vehicle_edge = getVehicleEdge( + path.points.at(i).point.pose, planner_data_->vehicle_info_.vehicle_width_m, + planner_data_->vehicle_info_.max_longitudinal_offset_m); + if (bg::intersects(toHybrid(to2D(line)), toHybrid(vehicle_edge))) { + first_idx_conflicting_lanelets = + std::min(first_idx_conflicting_lanelets, static_cast(i)); + is_conflict = true; + break; + } + } + } + if (is_conflict) { + return first_idx_conflicting_lanelets; + } else { + return boost::none; + } +} + +bool BlindSpotModule::generateStopLine( + const lanelet::ConstLanelets straight_lanelets, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx, + int * pass_judge_line_idx) const +{ + /* set judge line dist */ + const double current_vel = planner_data_->current_velocity->twist.linear.x; + const double max_acc = planner_data_->max_stop_acceleration_threshold; + const double delay_response_time = planner_data_->delay_response_time; + const double pass_judge_line_dist = + planning_utils::calcJudgeLineDistWithAccLimit(current_vel, max_acc, delay_response_time); + + /* set parameters */ + constexpr double interval = 0.2; + const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interval); + const int base2front_idx_dist = + std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); + const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); + + /* spline interpolation */ + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!util::splineInterpolate(*path, interval, &path_ip, logger_)) { + return false; + } + debug_data_.spline_path = path_ip; + + /* generate stop point */ + int stop_idx_ip; // stop point index for interpolated path. + if (straight_lanelets.size() > 0) { + boost::optional first_idx_conflicting_lane_opt = + getFirstPointConflictingLanelets(path_ip, straight_lanelets); + if (!first_idx_conflicting_lane_opt) { + RCLCPP_DEBUG(logger_, "No conflicting line found."); + return false; + } + stop_idx_ip = + std::max(first_idx_conflicting_lane_opt.get() - 1 - margin_idx_dist - base2front_idx_dist, 0); + } else { + boost::optional intersection_enter_point_opt = + getStartPointFromLaneLet(lane_id_); + if (!intersection_enter_point_opt) { + RCLCPP_DEBUG(logger_, "No intersection enter point found."); + return false; + } + planning_utils::calcClosestIndex( + path_ip, intersection_enter_point_opt.get(), stop_idx_ip, 10.0); + stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); + } + + /* insert stop_point */ + *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path); + + /* if another stop point exist before intersection stop_line, disable judge_line. */ + bool has_prior_stopline = false; + for (int i = 0; i < *stop_line_idx; ++i) { + if (std::fabs(path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { + has_prior_stopline = true; + break; + } + } + + /* insert judge point */ + const int pass_judge_idx_ip = std::min( + static_cast(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0)); + if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { + *pass_judge_line_idx = *stop_line_idx; + } else { + *pass_judge_line_idx = insertPoint(pass_judge_idx_ip, path_ip, path); + ++(*stop_line_idx); // stop index is incremented by judge line insertion + } + + RCLCPP_DEBUG( + logger_, + "generateStopLine() : stop_idx = %d, pass_judge_idx = %d, stop_idx_ip = %d, " + "pass_judge_idx_ip = %d, has_prior_stopline = %d", + *stop_line_idx, *pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); + + return true; +} + +void BlindSpotModule::cutPredictPathWithDuration( + autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const +{ + const rclcpp::Time current_time = clock_->now(); + + for (auto & object : objects_ptr->objects) { // each objects + for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = + rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); + } + } + } + } +} + +int BlindSpotModule::insertPoint( + const int insert_idx_ip, const autoware_auto_planning_msgs::msg::PathWithLaneId path_ip, + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path) const +{ + double insert_point_s = 0.0; + for (int i = 1; i <= insert_idx_ip; i++) { + insert_point_s += planning_utils::calcDist2d( + path_ip.points[i].point.pose.position, path_ip.points[i - 1].point.pose.position); + } + int insert_idx = -1; + // initialize with epsilon so that comparison with insert_point_s = 0.0 would work + double accum_s = 1e-6; + for (size_t i = 1; i < inout_path->points.size(); i++) { + accum_s += planning_utils::calcDist2d( + inout_path->points[i].point.pose.position, inout_path->points[i - 1].point.pose.position); + if (accum_s > insert_point_s) { + insert_idx = i; + break; + } + } + if (insert_idx >= 0) { + const auto it = inout_path->points.begin() + insert_idx; + autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point; + // copy from previous point + inserted_point = inout_path->points.at(std::max(insert_idx - 1, 0)); + inserted_point.point.pose = path_ip.points[insert_idx_ip].point.pose; + inout_path->points.insert(it, inserted_point); + } + return insert_idx; +} + +bool BlindSpotModule::checkObstacleInBlindSpot( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const +{ + /* get detection area */ + if (turn_direction_ == TurnDirection::INVALID) { + RCLCPP_WARN(logger_, "blind spot detector is running, turn_direction_ = not right or left."); + return false; + } + + const auto areas_opt = generateBlindSpotPolygons( + lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); + if (!!areas_opt) { + debug_data_.detection_area_for_blind_spot = areas_opt.get().detection_area; + debug_data_.conflict_area_for_blind_spot = areas_opt.get().conflict_area; + + autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; + cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); + + // check objects in blind spot areas + bool obstacle_detected = false; + for (const auto & object : objects.objects) { + if (!isTargetObjectType(object)) { + continue; + } + + bool exist_in_detection_area = bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(areas_opt.get().detection_area)); + bool exist_in_conflict_area = isPredictedPathInArea(object, areas_opt.get().conflict_area); + if (exist_in_detection_area && exist_in_conflict_area) { + obstacle_detected = true; + debug_data_.conflicting_targets.objects.push_back(object); + } + } + return obstacle_detected; + } else { + return false; + } +} + +bool BlindSpotModule::isPredictedPathInArea( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const lanelet::CompoundPolygon3d & area) const +{ + bool exist_in_conflict_area = false; + for (const auto & predicted_path : object.kinematics.predicted_paths) { + for (const auto & predicted_point : predicted_path.path) { + exist_in_conflict_area = + bg::within(to_bg2d(predicted_point.position), lanelet::utils::to2D(area)); + if (exist_in_conflict_area) { + return true; + } + } + } + return false; +} + +lanelet::ConstLanelet BlindSpotModule::generateHalfLanelet( + const lanelet::ConstLanelet lanelet) const +{ + lanelet::Points3d lefts, rights; + + const double offset = (turn_direction_ == TurnDirection::LEFT) + ? planner_param_.ignore_width_from_center_line + : -planner_param_.ignore_width_from_center_line; + const auto offset_centerline = lanelet::utils::getCenterlineWithOffset(lanelet, offset); + + const auto original_left_bound = + (turn_direction_ == TurnDirection::LEFT) ? lanelet.leftBound() : offset_centerline; + const auto original_right_bound = + (turn_direction_ == TurnDirection::LEFT) ? offset_centerline : lanelet.rightBound(); + + for (const auto & pt : original_left_bound) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : original_right_bound) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); + auto half_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto centerline = lanelet::utils::generateFineCenterline(half_lanelet, 5.0); + half_lanelet.setCenterline(centerline); + return std::move(half_lanelet); +} + +boost::optional BlindSpotModule::generateBlindSpotPolygons( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const geometry_msgs::msg::Pose & stop_line_pose) const +{ + std::vector lane_ids; + lanelet::ConstLanelets blind_spot_lanelets; + /* get lane ids until intersection */ + for (const auto & point : path.points) { + lane_ids.push_back(point.lane_ids.front()); + if (point.lane_ids.front() == lane_id_) { + break; + } + } + /* remove adjacent duplicates */ + lane_ids.erase(std::unique(lane_ids.begin(), lane_ids.end()), lane_ids.end()); + + /* reverse lane ids */ + std::reverse(lane_ids.begin(), lane_ids.end()); + + /* add intersection lanelet */ + const auto first_lanelet = lanelet_map_ptr->laneletLayer.get(lane_ids.front()); + const auto first_half_lanelet = generateHalfLanelet(first_lanelet); + blind_spot_lanelets.push_back(first_half_lanelet); + + if (lane_ids.size() > 1) { + for (size_t i = 0; i < lane_ids.size() - 1; ++i) { + const auto prev_lanelet = lanelet_map_ptr->laneletLayer.get(lane_ids.at(i)); + const auto next_lanelet = lanelet_map_ptr->laneletLayer.get(lane_ids.at(i + 1)); + /* end if next lanelet does not follow prev lanelet */ + if (!lanelet::geometry::follows(prev_lanelet.invert(), next_lanelet.invert())) { + break; + } + const auto half_lanelet = generateHalfLanelet(next_lanelet); + blind_spot_lanelets.push_back(half_lanelet); + } + /* reset order of lanelets */ + std::reverse(blind_spot_lanelets.begin(), blind_spot_lanelets.end()); + } + + const auto current_arc = + lanelet::utils::getArcCoordinates(blind_spot_lanelets, path.points[closest_idx].point.pose); + const auto stop_line_arc = lanelet::utils::getArcCoordinates(blind_spot_lanelets, stop_line_pose); + const auto total_length = lanelet::utils::getLaneletLength3d(blind_spot_lanelets); + const auto intersection_length = + lanelet::utils::getLaneletLength3d(lanelet_map_ptr->laneletLayer.get(lane_id_)); + const auto detection_area_start_length = + total_length - intersection_length - planner_param_.backward_length; + if ( + detection_area_start_length < current_arc.length && current_arc.length < stop_line_arc.length) { + const auto conflict_area = lanelet::utils::getPolygonFromArcLength( + blind_spot_lanelets, current_arc.length, stop_line_arc.length); + const auto detection_area = lanelet::utils::getPolygonFromArcLength( + blind_spot_lanelets, detection_area_start_length, current_arc.length); + + BlindSpotPolygons blind_spot_polygons; + blind_spot_polygons.conflict_area = conflict_area; + blind_spot_polygons.detection_area = detection_area; + + return blind_spot_polygons; + } else { + return boost::none; + } +} + +lanelet::LineString2d BlindSpotModule::getVehicleEdge( + const geometry_msgs::msg::Pose & vehicle_pose, const double vehicle_width, + const double base_link2front) const +{ + lanelet::LineString2d vehicle_edge; + tf2::Vector3 front_left, front_right, rear_left, rear_right; + front_left.setValue(base_link2front, vehicle_width / 2, 0); + front_right.setValue(base_link2front, -vehicle_width / 2, 0); + rear_left.setValue(0, vehicle_width / 2, 0); + rear_right.setValue(0, -vehicle_width / 2, 0); + + tf2::Transform tf; + tf2::fromMsg(vehicle_pose, tf); + const auto front_left_transformed = tf * front_left; + const auto front_right_transformed = tf * front_right; + const auto rear_left_transformed = tf * rear_left; + const auto rear_right_transformed = tf * rear_right; + + if (turn_direction_ == TurnDirection::LEFT) { + vehicle_edge.push_back( + lanelet::Point2d(0, front_left_transformed.x(), front_left_transformed.y())); + vehicle_edge.push_back( + lanelet::Point2d(0, rear_left_transformed.x(), rear_left_transformed.y())); + } else if (turn_direction_ == TurnDirection::RIGHT) { + vehicle_edge.push_back( + lanelet::Point2d(0, front_right_transformed.x(), front_right_transformed.y())); + vehicle_edge.push_back( + lanelet::Point2d(0, rear_right_transformed.x(), rear_right_transformed.y())); + } + return vehicle_edge; +} + +bool BlindSpotModule::isTargetObjectType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + +boost::optional BlindSpotModule::getStartPointFromLaneLet( + const int lane_id) const +{ + lanelet::ConstLanelet lanelet = planner_data_->lanelet_map->laneletLayer.get(lane_id); + if (lanelet.centerline().empty()) { + return boost::none; + } + const auto p = lanelet.centerline().front(); + geometry_msgs::msg::Point start_point; + start_point.x = p.x(); + start_point.y = p.y(); + start_point.z = p.z(); + + return start_point; +} + +lanelet::ConstLanelets BlindSpotModule::getStraightLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const int lane_id) +{ + lanelet::ConstLanelets straight_lanelets; + const auto intersection_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); + const auto prev_intersection_lanelets = routing_graph_ptr->previous(intersection_lanelet); + if (prev_intersection_lanelets.empty()) { + return straight_lanelets; + } + + const auto next_lanelets = routing_graph_ptr->following(prev_intersection_lanelets.front()); + for (const auto & ll : next_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (!turn_direction.compare("straight")) { + straight_lanelets.push_back(ll); + } + } + return straight_lanelets; +} + +void BlindSpotModule::StateMachine::setStateWithMarginTime( + State state, rclcpp::Logger logger, rclcpp::Clock & clock) +{ + /* same state request */ + if (state_ == state) { + start_time_ = nullptr; // reset timer + return; + } + + /* GO -> STOP */ + if (state == State::STOP) { + state_ = State::STOP; + start_time_ = nullptr; // reset timer + return; + } + + /* STOP -> GO */ + if (state == State::GO) { + if (start_time_ == nullptr) { + start_time_ = std::make_shared(clock.now()); + } else { + const double duration = (clock.now() - *start_time_).seconds(); + if (duration > margin_time_) { + state_ = State::GO; + start_time_ = nullptr; // reset timer + } + } + return; + } + + RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); +} + +void BlindSpotModule::StateMachine::setState(State state) { state_ = state; } + +void BlindSpotModule::StateMachine::setMarginTime(const double t) { margin_time_ = t; } + +BlindSpotModule::State BlindSpotModule::StateMachine::getState() { return state_; } +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp new file mode 100644 index 0000000000000..2cc54db1273cb --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/debug.cpp @@ -0,0 +1,538 @@ +// Copyright 2020 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 +#include +#include +#include + +#include + +namespace behavior_velocity_planner +{ +namespace +{ +visualization_msgs::msg::MarkerArray createCrosswalkMarkers( + const DebugData & debug_data, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + tf2::Transform tf_base_link2front( + tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0)); + + // Crosswalk polygons + int32_t uid = planning_utils::bitShift(module_id); + for (size_t i = 0; i < debug_data.crosswalk_polygons.size(); ++i) { + std::vector polygon = debug_data.crosswalk_polygons.at(i); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + + marker.ns = "crosswalk polygon line"; + marker.id = uid + i; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.1; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + for (size_t j = 0; j < polygon.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = polygon.at(j).x(); + point.y = polygon.at(j).y(); + point.z = polygon.at(j).z(); + marker.points.push_back(point); + } + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + + marker.ns = "crosswalk polygon point"; + marker.id = i; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.25; + marker.scale.y = 0.25; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + for (size_t j = 0; j < polygon.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = polygon.at(j).x(); + point.y = polygon.at(j).y(); + point.z = polygon.at(j).z(); + marker.points.push_back(point); + } + msg.markers.push_back(marker); + } + + // Collision line + for (size_t i = 0; i < debug_data.collision_lines.size(); ++i) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "collision line"; + marker.id = uid + i; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.1; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + for (size_t j = 0; j < debug_data.collision_lines.at(i).size(); ++j) { + geometry_msgs::msg::Point point; + point.x = debug_data.collision_lines.at(i).at(j).x(); + point.y = debug_data.collision_lines.at(i).at(j).y(); + point.z = debug_data.collision_lines.at(i).at(j).z(); + marker.points.push_back(point); + } + msg.markers.push_back(marker); + } + + // Collision point + if (!debug_data.collision_points.empty()) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "collision point"; + marker.id = 0; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.25; + marker.scale.y = 0.25; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + for (size_t j = 0; j < debug_data.collision_points.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = debug_data.collision_points.at(j).x(); + point.y = debug_data.collision_points.at(j).y(); + point.z = debug_data.collision_points.at(j).z(); + marker.points.push_back(point); + } + msg.markers.push_back(marker); + } + + // Slow polygon + for (size_t i = 0; i < debug_data.slow_polygons.size(); ++i) { + std::vector polygon = debug_data.slow_polygons.at(i); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + + marker.ns = "slow polygon line"; + marker.id = uid + i; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.1; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + for (size_t j = 0; j < polygon.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = polygon.at(j).x(); + point.y = polygon.at(j).y(); + point.z = polygon.at(j).z(); + marker.points.push_back(point); + } + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + + // Slow point + if (!debug_data.slow_poses.empty()) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "slow point"; + marker.id = 0; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.25; + marker.scale.y = 0.25; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + for (size_t j = 0; j < debug_data.slow_poses.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = debug_data.slow_poses.at(j).position.x; + point.y = debug_data.slow_poses.at(j).position.y; + point.z = debug_data.slow_poses.at(j).position.z; + marker.points.push_back(point); + } + msg.markers.push_back(marker); + } + + // Stop polygon + for (size_t i = 0; i < debug_data.stop_polygons.size(); ++i) { + std::vector polygon = debug_data.stop_polygons.at(i); + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + + marker.ns = "stop polygon line"; + marker.id = uid + i; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.1; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + for (size_t j = 0; j < polygon.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = polygon.at(j).x(); + point.y = polygon.at(j).y(); + point.z = polygon.at(j).z(); + marker.points.push_back(point); + } + marker.points.push_back(marker.points.front()); + msg.markers.push_back(marker); + } + + // Stop point + if (!debug_data.stop_poses.empty()) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "stop point"; + marker.id = module_id; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.25; + marker.scale.y = 0.25; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = debug_data.stop_poses.at(j).position.x; + point.y = debug_data.stop_poses.at(j).position.y; + point.z = debug_data.stop_poses.at(j).position.z; + marker.points.push_back(point); + } + msg.markers.push_back(marker); + } + + // Stop VirtualWall + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "stop_virtual_wall"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + // Factor Text + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "factor_text"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "crosswalk"; + msg.markers.push_back(marker); + } + + // Slow VirtualWall + for (size_t j = 0; j < debug_data.slow_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "slow virtual_wall"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.slow_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + // Slow Factor Text + for (size_t j = 0; j < debug_data.slow_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "slow factor_text"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.slow_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "crosswalk"; + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createWalkwayMarkers( + const DebugData & debug_data, const int64_t module_id) +{ + int32_t uid = planning_utils::bitShift(module_id); + visualization_msgs::msg::MarkerArray msg; + tf2::Transform tf_base_link2front( + tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0)); + + // Stop point + if (!debug_data.stop_poses.empty()) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "stop point"; + marker.id = module_id; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::POINTS; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; + marker.pose.position.y = 0; + marker.pose.position.z = 0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + marker.scale.x = 0.25; + marker.scale.y = 0.25; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + geometry_msgs::msg::Point point; + point.x = debug_data.stop_poses.at(j).position.x; + point.y = debug_data.stop_poses.at(j).position.y; + point.z = debug_data.stop_poses.at(j).position.z; + marker.points.push_back(point); + } + msg.markers.push_back(marker); + } + + // Stop VirtualWall + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "stop_virtual_wall"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + + visualization_msgs::msg::Marker range_marker; + range_marker.header.frame_id = "map"; + range_marker.ns = "walkway stop judge range"; + range_marker.id = uid + debug_data.stop_poses.size() + j; + range_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + range_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + range_marker.action = visualization_msgs::msg::Marker::ADD; + range_marker.pose.position.x = 0; + range_marker.pose.position.y = 0; + range_marker.pose.position.z = 0; + range_marker.pose.orientation.x = 0.0; + range_marker.pose.orientation.y = 0.0; + range_marker.pose.orientation.z = 0.0; + range_marker.pose.orientation.w = 1.0; + range_marker.scale.x = 0.1; + range_marker.scale.y = 0.1; + range_marker.color.a = 0.5; // Don't forget to set the alpha! + range_marker.color.r = 1.0; + range_marker.color.g = 0.0; + range_marker.color.b = 0.0; + geometry_msgs::msg::Point point; + point.x = debug_data.stop_poses.at(j).position.x; + point.y = debug_data.stop_poses.at(j).position.y; + point.z = debug_data.stop_poses.at(j).position.z; + for (size_t i = 0; i < 50; ++i) { + geometry_msgs::msg::Point range_point; + range_point.x = point.x + debug_data.stop_judge_range * std::cos(M_PI * 2 / 50 * i); + range_point.y = point.y + debug_data.stop_judge_range * std::sin(M_PI * 2 / 50 * i); + range_point.z = point.z; + range_marker.points.push_back(range_point); + } + range_marker.points.push_back(range_marker.points.front()); + msg.markers.push_back(range_marker); + } + // Factor Text + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "factor_text"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "walkway"; + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace + +visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + appendMarkerArray( + createCrosswalkMarkers(debug_data_, module_id_), this->clock_->now(), &debug_marker_array); + + return debug_marker_array; +} + +visualization_msgs::msg::MarkerArray WalkwayModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + appendMarkerArray( + createWalkwayMarkers(debug_data_, module_id_), this->clock_->now(), &debug_marker_array); + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp new file mode 100644 index 0000000000000..d4425eb8ff917 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/manager.cpp @@ -0,0 +1,122 @@ +// Copyright 2020 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 + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::vector getCrosswalksOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::vector crosswalks; + + std::set unique_lane_ids; + for (const auto & p : path.points) { + unique_lane_ids.insert(p.lane_ids.at(0)); // should we iterate ids? keep as it was. + } + + for (const auto & lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + constexpr int PEDESTRIAN_GRAPH_ID = 1; + const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); + for (const auto & crosswalk : conflicting_crosswalks) { + crosswalks.push_back(crosswalk); + } + } + + return crosswalks; +} + +std::set getCrosswalkIdSetOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map, + const std::shared_ptr & overall_graphs) +{ + std::set crosswalk_id_set; + + for (const auto & crosswalk : getCrosswalksOnPath(path, lanelet_map, overall_graphs)) { + crosswalk_id_set.insert(crosswalk.id()); + } + + return crosswalk_id_set; +} + +} // namespace + +CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + + // for crosswalk parameters + auto & cp = crosswalk_planner_param_; + cp.stop_line_distance = node.declare_parameter(ns + ".crosswalk.stop_line_distance", 1.5); + cp.stop_margin = node.declare_parameter(ns + ".crosswalk.stop_margin", 1.0); + cp.slow_margin = node.declare_parameter(ns + ".crosswalk.slow_margin", 2.0); + cp.slow_velocity = node.declare_parameter(ns + ".crosswalk.slow_velocity", 5.0 / 3.6); + cp.stop_predicted_object_prediction_time_margin = + node.declare_parameter(ns + ".crosswalk.stop_predicted_object_prediction_time_margin", 3.0); + cp.external_input_timeout = node.declare_parameter(ns + ".crosswalk.external_input_timeout", 1.0); + + // for walkway parameters + auto & wp = walkway_planner_param_; + wp.stop_margin = node.declare_parameter(ns + ".walkway.stop_margin", 1.0); + wp.stop_line_distance = node.declare_parameter(ns + ".walkway.stop_line_distance", 1.0); + wp.stop_duration_sec = node.declare_parameter(ns + ".walkway.stop_duration_sec", 1.0); + wp.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); +} + +void CrosswalkModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & crosswalk : + getCrosswalksOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs)) { + const auto module_id = crosswalk.id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, crosswalk, crosswalk_planner_param_, logger_.get_child("crosswalk_module"), + clock_)); + if ( + crosswalk.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")) == + lanelet::AttributeValueString::Walkway) { + registerModule(std::make_shared( + module_id, crosswalk, walkway_planner_param_, logger_.get_child("walkway_module"), + clock_)); + } + } + } +} + +std::function &)> +CrosswalkModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto crosswalk_id_set = + getCrosswalkIdSetOnPath(path, planner_data_->lanelet_map, planner_data_->overall_graphs); + + return [crosswalk_id_set](const std::shared_ptr & scene_module) { + return crosswalk_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp new file mode 100644 index 0000000000000..f9b04b2a88e6a --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -0,0 +1,392 @@ +// Copyright 2020 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 +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +CrosswalkModule::CrosswalkModule( + const int64_t module_id, const lanelet::ConstLanelet & crosswalk, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + module_id_(module_id), + crosswalk_(crosswalk), + state_(State::APPROACH) +{ + planner_param_ = planner_param; +} + +bool CrosswalkModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + first_stop_path_point_index_ = static_cast(path->points.size()) - 1; + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::CROSSWALK); + + const auto input = *path; + + // create polygon + lanelet::CompoundPolygon3d lanelet_polygon = crosswalk_.polygon3d(); + Polygon polygon; + for (const auto & lanelet_point : lanelet_polygon) { + polygon.outer().push_back(bg::make(lanelet_point.x(), lanelet_point.y())); + } + polygon.outer().push_back(polygon.outer().front()); + polygon = isClockWise(polygon) ? polygon : inverseClockWise(polygon); + + // check state + geometry_msgs::msg::PoseStamped self_pose = planner_data_->current_pose; + if (bg::within(Point(self_pose.pose.position.x, self_pose.pose.position.y), polygon)) { + state_ = State::INSIDE; + } else if (state_ == State::INSIDE) { + state_ = State::GO_OUT; + } + + if (state_ == State::APPROACH) { + // check person in polygon + const auto objects_ptr = planner_data_->predicted_objects; + const auto no_ground_pointcloud_ptr = planner_data_->no_ground_pointcloud; + + autoware_auto_planning_msgs::msg::PathWithLaneId slow_path, stop_path; + if (!checkSlowArea(input, polygon, objects_ptr, no_ground_pointcloud_ptr, slow_path)) { + return false; + } + + bool insert_stop; + if (!checkStopArea( + slow_path, polygon, objects_ptr, no_ground_pointcloud_ptr, stop_path, &insert_stop)) { + return false; + } + // stop_path = slow_path; + *path = stop_path; + + if (insert_stop) { + /* get stop point and stop factor */ + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.first_stop_pose; + stop_factor.stop_factor_points = debug_data_.stop_factor_points; + planning_utils::appendStopReason(stop_factor, stop_reason); + } + } + return true; +} + +bool CrosswalkModule::checkStopArea( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const Polygon & crosswalk_polygon, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & objects_ptr, + [[maybe_unused]] const pcl::PointCloud::ConstPtr & no_ground_pointcloud_ptr, + autoware_auto_planning_msgs::msg::PathWithLaneId & output, bool * insert_stop) +{ + output = input; + *insert_stop = false; + bool pedestrian_found = false; + bool object_found = false; + const bool external_slowdown = + isTargetExternalInputStatus(autoware_api_msgs::msg::CrosswalkStatus::SLOWDOWN); + const bool external_stop = + isTargetExternalInputStatus(autoware_api_msgs::msg::CrosswalkStatus::STOP); + const bool external_go = isTargetExternalInputStatus(autoware_api_msgs::msg::CrosswalkStatus::GO); + rclcpp::Time current_time = clock_->now(); + + // create stop area + Polygon stop_polygon; + if (!createVehiclePathPolygonInCrosswalk(output, crosswalk_polygon, 1.0, stop_polygon)) { + return false; + } + + // -- debug code -- + std::vector points; + for (size_t i = 0; i < stop_polygon.outer().size(); ++i) { + const auto p = stop_polygon.outer().at(i); + points.push_back(Eigen::Vector3d(p.x(), p.y(), planner_data_->current_pose.pose.position.z)); + } + debug_data_.stop_polygons.push_back(points); + // ---------------- + + // check object pointcloud + // for (size_t i = 0; i < no_ground_pointcloud_ptr->size(); ++i) { + // Point point(no_ground_pointcloud_ptr->at(i).x, no_ground_pointcloud_ptr->at(i).y); + // if (!bg::within(point, crosswalk_polygon)) continue; + // if (bg::within(point, stop_polygon)) { + // object_found = true; + // debug_data_.stop_factor_points.emplace_back( + // planning_utils::toRosPoint(no_ground_pointcloud_ptr->at(i))); + // break; + // } + // } + + // check pedestrian + for (const auto & object : objects_ptr->objects) { + if (object_found) { + break; + } + + if (isTargetType(object)) { + Point point( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + if (!bg::within(point, crosswalk_polygon)) { + continue; + } + if (bg::within(point, stop_polygon)) { + pedestrian_found = true; + debug_data_.stop_factor_points.emplace_back( + object.kinematics.initial_pose_with_covariance.pose.position); + break; + } + for (const auto & object_path : object.kinematics.predicted_paths) { + for (size_t k = 0; k < object_path.path.size() - 1; ++k) { + const auto predicted_time = + rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Duration(object_path.time_step) * static_cast(k); + if ( + (predicted_time - current_time).seconds() < + planner_param_.stop_predicted_object_prediction_time_margin) { + const auto op0 = object_path.path.at(k).position; + const auto op1 = object_path.path.at(k + 1).position; + const Line line{{op0.x, op0.y}, {op1.x, op1.y}}; + std::vector line_collision_points; + bg::intersection(stop_polygon, line, line_collision_points); + if (!line_collision_points.empty()) { + pedestrian_found = true; + } + if (pedestrian_found) { + debug_data_.stop_factor_points.emplace_back( + object.kinematics.initial_pose_with_covariance.pose.position); + break; + } + } + } + } + } + } + + bool stop = false; + + if (pedestrian_found || object_found) { + stop = true; + } + if (external_go || external_slowdown) { + stop = false; + } else if (external_stop) { + stop = true; + } + + // insert stop point + if (stop) { + lanelet::Optional stop_line_opt = + getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); + if (!!stop_line_opt) { + if (!insertTargetVelocityPoint( + input, stop_line_opt.get(), planner_param_.stop_margin, 0.0, *planner_data_, output, + debug_data_, first_stop_path_point_index_)) { + return false; + } + } else { + if (!insertTargetVelocityPoint( + input, crosswalk_polygon, + planner_param_.stop_line_distance + planner_param_.stop_margin, 0.0, *planner_data_, + output, debug_data_, first_stop_path_point_index_)) { + return false; + } + *insert_stop = stop; + } + } + return true; +} + +bool CrosswalkModule::checkSlowArea( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const Polygon & crosswalk_polygon, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & objects_ptr, + [[maybe_unused]] const pcl::PointCloud::ConstPtr & no_ground_pointcloud_ptr, + autoware_auto_planning_msgs::msg::PathWithLaneId & output) +{ + output = input; + bool pedestrian_found = false; + const bool external_slowdown = + isTargetExternalInputStatus(autoware_api_msgs::msg::CrosswalkStatus::SLOWDOWN); + const bool external_stop = + isTargetExternalInputStatus(autoware_api_msgs::msg::CrosswalkStatus::STOP); + const bool external_go = isTargetExternalInputStatus(autoware_api_msgs::msg::CrosswalkStatus::GO); + + Polygon slowdown_polygon; + if (!createVehiclePathPolygonInCrosswalk(output, crosswalk_polygon, 4.0, slowdown_polygon)) { + return false; + } + + // -- debug code -- + std::vector points; + for (size_t i = 0; i < slowdown_polygon.outer().size(); ++i) { + Eigen::Vector3d point; + point << slowdown_polygon.outer().at(i).x(), slowdown_polygon.outer().at(i).y(), + planner_data_->current_pose.pose.position.z; + points.push_back(point); + } + debug_data_.slow_polygons.push_back(points); + // ---------------- + + // check pedestrian + for (const auto & object : objects_ptr->objects) { + if (isTargetType(object)) { + Point point( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + if (!bg::within(point, crosswalk_polygon)) { + continue; + } + if (bg::within(point, slowdown_polygon)) { + pedestrian_found = true; + } + } + } + + bool slowdown = false; + + if (pedestrian_found) { + slowdown = true; + } + if (external_go || external_stop) { + slowdown = false; + } else if (external_slowdown) { + slowdown = true; + } + + // insert slow point + if (slowdown) { + lanelet::Optional stop_line_opt = + getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); + if (!!stop_line_opt) { + if (!insertTargetVelocityPoint( + input, stop_line_opt.get(), planner_param_.slow_margin, planner_param_.slow_velocity, + *planner_data_, output, debug_data_, first_stop_path_point_index_)) { + return false; + } + } else { + if (!insertTargetVelocityPoint( + input, slowdown_polygon, planner_param_.stop_line_distance + planner_param_.slow_margin, + planner_param_.slow_velocity, *planner_data_, output, debug_data_, + first_stop_path_point_index_)) { + return false; + } + } + } + return true; +} +bool CrosswalkModule::createVehiclePathPolygonInCrosswalk( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const Polygon & crosswalk_polygon, + const float extended_width, Polygon & path_polygon) +{ + std::vector path_collision_points; + for (size_t i = 0; i < input.points.size() - 1; ++i) { + const auto p0 = input.points.at(i).point.pose.position; + const auto p1 = input.points.at(i + 1).point.pose.position; + const Line line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector line_collision_points; + bg::intersection(crosswalk_polygon, line, line_collision_points); + if (line_collision_points.empty()) { + continue; + } + for (size_t j = 0; j < line_collision_points.size(); ++j) { + path_collision_points.push_back(line_collision_points.at(j)); + } + } + if (path_collision_points.size() != 2) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 5000, + "There must be two points of conflict between the crosswalk polygon and the path. points is " + "%d", + (int)path_collision_points.size()); + return false; + } + + Polygon candidate_path_polygon; + { + const auto cp0 = path_collision_points.at(0); + const auto cp1 = path_collision_points.at(1); + + const double vehicle_width = planner_data_->vehicle_info_.vehicle_width_m; + const double polygon_width = (vehicle_width / 2.0) + extended_width; + const double polygon_extend_length = std::hypot(cp0.x() - cp1.x(), cp0.y() - cp1.y()); + + const double yaw = std::atan2(cp1.y() - cp0.y(), cp1.x() - cp0.x()); + const double w_cos = polygon_width * std::cos(yaw); + const double w_sin = polygon_width * std::sin(yaw); + const double l_cos = polygon_extend_length * std::cos(yaw); + const double l_sin = polygon_extend_length * std::sin(yaw); + + // The candidate_path_polygon is generated to be + // a constant width(polygon_width) from the centerline of the path + // across the crosswalk. The length of the candidate_path_polygon + // is also extended so that the four corners of the polygon + // are not inside the crosswalk polygon. + candidate_path_polygon.outer().push_back( + bg::make(cp0.x() + w_sin - l_cos, cp0.y() - w_cos - l_sin)); + candidate_path_polygon.outer().push_back( + bg::make(cp0.x() - w_sin - l_cos, cp0.y() + w_cos - l_sin)); + candidate_path_polygon.outer().push_back( + bg::make(cp1.x() - w_sin + l_cos, cp1.y() + w_cos + l_sin)); + candidate_path_polygon.outer().push_back( + bg::make(cp1.x() + w_sin + l_cos, cp1.y() - w_cos + l_sin)); + candidate_path_polygon.outer().push_back(candidate_path_polygon.outer().front()); + } + candidate_path_polygon = isClockWise(candidate_path_polygon) + ? candidate_path_polygon + : inverseClockWise(candidate_path_polygon); + + std::vector path_polygons; + bg::intersection(crosswalk_polygon, candidate_path_polygon, path_polygons); + + if (path_polygons.size() != 1) { + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 5000, "Number of polygon is %d. Must be 1", (int)path_polygons.size()); + return false; + } + path_polygon = path_polygons.at(0); + return true; +} + +bool CrosswalkModule::isTargetType(const autoware_auto_perception_msgs::msg::PredictedObject & obj) +{ + if ( + obj.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || + obj.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { + return true; + } + return false; +} + +bool CrosswalkModule::isTargetExternalInputStatus(const int target_status) +{ + return planner_data_->external_crosswalk_status_input && + planner_data_->external_crosswalk_status_input.get().status == target_status && + (clock_->now() - planner_data_->external_crosswalk_status_input.get().header.stamp) + .seconds() < planner_param_.external_input_timeout; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp new file mode 100644 index 0000000000000..40e9cf3996449 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp @@ -0,0 +1,105 @@ +// Copyright 2020 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 +#include + +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +WalkwayModule::WalkwayModule( + const int64_t module_id, const lanelet::ConstLanelet & walkway, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + module_id_(module_id), + walkway_(walkway), + state_(State::APPROACH) +{ + planner_param_ = planner_param; +} + +bool WalkwayModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + first_stop_path_point_index_ = static_cast(path->points.size()) - 1; + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::WALKWAY); + + const auto input = *path; + + if (state_ == State::APPROACH) { + // create polygon + lanelet::CompoundPolygon3d lanelet_polygon = walkway_.polygon3d(); + Polygon polygon; + for (const auto & lanelet_point : lanelet_polygon) { + polygon.outer().push_back(bg::make(lanelet_point.x(), lanelet_point.y())); + } + polygon.outer().push_back(polygon.outer().front()); + polygon = isClockWise(polygon) ? polygon : inverseClockWise(polygon); + + lanelet::Optional stop_line_opt = + getStopLineFromMap(module_id_, planner_data_, "crosswalk_id"); + if (!!stop_line_opt) { + if (!insertTargetVelocityPoint( + input, stop_line_opt.get(), planner_param_.stop_margin, 0.0, *planner_data_, *path, + debug_data_, first_stop_path_point_index_)) { + return false; + } + } else { + if (!insertTargetVelocityPoint( + input, polygon, planner_param_.stop_line_distance + planner_param_.stop_margin, 0.0, + *planner_data_, *path, debug_data_, first_stop_path_point_index_)) { + return false; + } + } + + /* get stop point and stop factor */ + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.first_stop_pose; + stop_factor.stop_factor_points.emplace_back(debug_data_.nearest_collision_point); + planning_utils::appendStopReason(stop_factor, stop_reason); + + // update state + const Point self_pose = { + planner_data_->current_pose.pose.position.x, planner_data_->current_pose.pose.position.y}; + const Point stop_pose = { + debug_data_.first_stop_pose.position.x, debug_data_.first_stop_pose.position.y}; + const double distance = bg::distance(stop_pose, self_pose); + const double distance_threshold = 1.0; + debug_data_.stop_judge_range = distance_threshold; + if ( + distance < distance_threshold && + planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { + state_ = State::STOP; + } + return true; + } else if (state_ == State::STOP) { + if (planner_data_->isVehicleStopped()) { + state_ = State::SURPASSED; + } + } else if (state_ == State::SURPASSED) { + } + return true; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp new file mode 100644 index 0000000000000..6fc8498a79d4a --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/util.cpp @@ -0,0 +1,339 @@ +// Copyright 2020 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 +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include +#include + +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; +using Point = bg::model::d2::point_xy; +using Polygon = bg::model::polygon; +using Line = bg::model::linestring; + +bool getBackwardPointFromBasePoint( + const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, + const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) +{ + Eigen::Vector2d line_vec = line_point2 - line_point1; + Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); + output_point = base_point + backward_vec; + return true; +} + +bool insertTargetVelocityPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const Polygon & polygon, + const double & margin, const double & velocity, const PlannerData & planner_data, + autoware_auto_planning_msgs::msg::PathWithLaneId & output, DebugData & debug_data, + boost::optional & first_stop_path_point_index) +{ + output = input; + for (size_t i = 0; i < output.points.size() - 1; ++i) { + const auto p0 = output.points.at(i).point.pose.position; + const auto p1 = output.points.at(i + 1).point.pose.position; + const Line line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + bg::intersection(polygon, line, collision_points); + + if (collision_points.empty()) { + continue; + } + // -- debug code -- + for (const auto & cp : collision_points) { + Eigen::Vector3d point3d(cp.x(), cp.y(), planner_data.current_pose.pose.position.z); + debug_data.collision_points.push_back(point3d); + } + std::vector line3d; + line3d.emplace_back(p0.x, p0.y, p0.z); + line3d.emplace_back(p1.x, p1.y, p1.z); + debug_data.collision_lines.push_back(line3d); + // ---------------- + + // check nearest collision point + Point nearest_collision_point{}; + double min_dist = 0.0; + for (size_t j = 0; j < collision_points.size(); ++j) { + double dist = bg::distance(Point(p0.x, p0.y), collision_points.at(j)); + if (j == 0 || dist < min_dist) { + min_dist = dist; + nearest_collision_point = collision_points.at(j); + debug_data.nearest_collision_point = + planning_utils::toRosPoint(collision_points.at(j), p0.z); + } + } + + // search target point index + size_t insert_target_point_idx = 0; + const double base_link2front = planner_data.vehicle_info_.max_longitudinal_offset_m; + double length_sum = 0; + + const double target_length = margin + base_link2front; + Eigen::Vector2d point1, point2; + point1 << nearest_collision_point.x(), nearest_collision_point.y(); + point2 << p0.x, p0.y; + length_sum += (point2 - point1).norm(); + for (size_t j = i; 0 < j; --j) { + if (target_length < length_sum) { + insert_target_point_idx = j + 1; + break; + } + const auto pj1 = output.points.at(j).point.pose.position; + const auto pj2 = output.points.at(j - 1).point.pose.position; + point1 << pj1.x, pj1.y; + point2 << pj2.x, pj2.y; + length_sum += (point2 - point1).norm(); + } + + // create target point + Eigen::Vector2d target_point; + autoware_auto_planning_msgs::msg::PathPointWithLaneId target_point_with_lane_id; + getBackwardPointFromBasePoint(point2, point1, point2, length_sum - target_length, target_point); + const int target_velocity_point_idx = + std::max(static_cast(insert_target_point_idx) - 1, 0); + target_point_with_lane_id = output.points.at(target_velocity_point_idx); + target_point_with_lane_id.point.pose.position.x = target_point.x(); + target_point_with_lane_id.point.pose.position.y = target_point.y(); + if (insert_target_point_idx > 0) { + // calculate z-position of the target point (Internal division point of point1/point2) + // if insert_target_point_idx is zero, use z-position of target_velocity_point_idx + const double internal_div_ratio = + (point1 - target_point).norm() / + ((point1 - target_point).norm() + (point2 - target_point).norm()); + target_point_with_lane_id.point.pose.position.z = + output.points.at(insert_target_point_idx).point.pose.position.z * (1 - internal_div_ratio) + + output.points.at(insert_target_point_idx - 1).point.pose.position.z * internal_div_ratio; + } + + if ((point1 - point2).norm() > 1.0E-3) { + const double yaw = std::atan2(point1.y() - point2.y(), point1.x() - point2.x()); + target_point_with_lane_id.point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); + } + target_point_with_lane_id.point.longitudinal_velocity_mps = velocity; + if (velocity == 0.0 && target_velocity_point_idx < first_stop_path_point_index) { + first_stop_path_point_index = target_velocity_point_idx; + // -- debug code -- + debug_data.first_stop_pose = target_point_with_lane_id.point.pose; + // ---------------- + } + // -- debug code -- + if (velocity == 0.0) { + debug_data.stop_poses.push_back(target_point_with_lane_id.point.pose); + } else { + debug_data.slow_poses.push_back(target_point_with_lane_id.point.pose); + } + // ---------------- + + // insert target point + output.points.insert( + output.points.begin() + insert_target_point_idx, target_point_with_lane_id); + + // insert 0 velocity after target point + for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) { + output.points.at(j).point.longitudinal_velocity_mps = + std::min(static_cast(velocity), output.points.at(j).point.longitudinal_velocity_mps); + } + return true; + } + return false; +} + +lanelet::Optional getStopLineFromMap( + const int lane_id, const std::shared_ptr & planner_data, + const std::string & attribute_name) +{ + lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + const auto road_markings = lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stop_line; + for (const auto & road_marking : road_markings) { + // TODO(someone): Create regulatory element for crosswalk + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + const int target_id = road_marking->roadMarking().attributeOr(attribute_name, 0); + if (type == lanelet::AttributeValueString::StopLine && target_id == lane_id) { + stop_line.push_back(road_marking->roadMarking()); + break; // only one stop_line exists. + } + } + if (stop_line.empty()) { + return {}; + } + + return stop_line.front(); +} + +bool insertTargetVelocityPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const lanelet::ConstLineString3d & stop_line, const double & margin, const double & velocity, + const PlannerData & planner_data, autoware_auto_planning_msgs::msg::PathWithLaneId & output, + DebugData & debug_data, boost::optional & first_stop_path_point_index) +{ + using lanelet::utils::to2D; + using lanelet::utils::toHybrid; + output = input; + for (size_t i = 0; i < output.points.size() - 1; ++i) { + const auto p0 = output.points.at(i).point.pose.position; + const auto p1 = output.points.at(i + 1).point.pose.position; + const Line line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + bg::intersection(toHybrid(to2D(stop_line)), line, collision_points); + + if (collision_points.empty()) { + continue; + } + // -- debug code -- + for (const auto & cp : collision_points) { + Eigen::Vector3d point3d(cp.x(), cp.y(), planner_data.current_pose.pose.position.z); + debug_data.collision_points.push_back(point3d); + } + std::vector line3d; + line3d.emplace_back(p0.x, p0.y, p0.z); + line3d.emplace_back(p1.x, p1.y, p1.z); + debug_data.collision_lines.push_back(line3d); + // ---------------- + + // check nearest collision point + Point nearest_collision_point{}; + double min_dist = 0.0; + for (size_t j = 0; j < collision_points.size(); ++j) { + double dist = bg::distance(Point(p0.x, p0.y), collision_points.at(j)); + if (j == 0 || dist < min_dist) { + min_dist = dist; + nearest_collision_point = collision_points.at(j); + debug_data.nearest_collision_point = + planning_utils::toRosPoint(collision_points.at(j), p0.z); + } + } + + // search target point index + size_t insert_target_point_idx = 0; + const double base_link2front = planner_data.vehicle_info_.max_longitudinal_offset_m; + double length_sum = 0; + + const double target_length = margin + base_link2front; + Eigen::Vector2d point1, point2; + point1 << nearest_collision_point.x(), nearest_collision_point.y(); + point2 << p0.x, p0.y; + length_sum += (point2 - point1).norm(); + for (size_t j = i; 0 < j; --j) { + if (target_length < length_sum) { + insert_target_point_idx = j + 1; + break; + } + const auto pj1 = output.points.at(j).point.pose.position; + const auto pj2 = output.points.at(j - 1).point.pose.position; + point1 << pj1.x, pj1.y; + point2 << pj2.x, pj2.y; + length_sum += (point2 - point1).norm(); + } + + // create target point + Eigen::Vector2d target_point; + autoware_auto_planning_msgs::msg::PathPointWithLaneId target_point_with_lane_id; + getBackwardPointFromBasePoint(point2, point1, point2, length_sum - target_length, target_point); + const int target_velocity_point_idx = + std::max(static_cast(insert_target_point_idx) - 1, 0); + target_point_with_lane_id = output.points.at(target_velocity_point_idx); + target_point_with_lane_id.point.pose.position.x = target_point.x(); + target_point_with_lane_id.point.pose.position.y = target_point.y(); + if (insert_target_point_idx > 0) { + // calculate z-position of the target point (Internal division point of point1/point2) + // if insert_target_point_idx is zero, use z-position of target_velocity_point_idx + const double internal_div_ratio = + (point1 - target_point).norm() / + ((point1 - target_point).norm() + (point2 - target_point).norm()); + target_point_with_lane_id.point.pose.position.z = + output.points.at(insert_target_point_idx).point.pose.position.z * (1 - internal_div_ratio) + + output.points.at(insert_target_point_idx - 1).point.pose.position.z * internal_div_ratio; + } + + if ((point1 - point2).norm() > 1.0E-3) { + const double yaw = std::atan2(point1.y() - point2.y(), point1.x() - point2.x()); + target_point_with_lane_id.point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); + } + target_point_with_lane_id.point.longitudinal_velocity_mps = velocity; + if (velocity == 0.0 && target_velocity_point_idx < first_stop_path_point_index) { + first_stop_path_point_index = target_velocity_point_idx; + // -- debug code -- + debug_data.first_stop_pose = target_point_with_lane_id.point.pose; + // ---------------- + } + // -- debug code -- + if (velocity == 0.0) { + debug_data.stop_poses.push_back(target_point_with_lane_id.point.pose); + } else { + debug_data.slow_poses.push_back(target_point_with_lane_id.point.pose); + } + // ---------------- + + // insert target point + output.points.insert( + output.points.begin() + insert_target_point_idx, target_point_with_lane_id); + + // insert 0 velocity after target point + for (size_t j = insert_target_point_idx; j < output.points.size(); ++j) { + output.points.at(j).point.longitudinal_velocity_mps = + std::min(static_cast(velocity), output.points.at(j).point.longitudinal_velocity_mps); + } + return true; + } + return false; +} + +bool isClockWise(const Polygon & polygon) +{ + const auto n = polygon.outer().size(); + + const double x_offset = polygon.outer().at(0).x(); + const double y_offset = polygon.outer().at(0).y(); + double sum = 0.0; + for (std::size_t i = 0; i < polygon.outer().size(); ++i) { + sum += + (polygon.outer().at(i).x() - x_offset) * (polygon.outer().at((i + 1) % n).y() - y_offset) - + (polygon.outer().at(i).y() - y_offset) * (polygon.outer().at((i + 1) % n).x() - x_offset); + } + return sum < 0.0; +} + +Polygon inverseClockWise(const Polygon & polygon) +{ + const auto & poly = polygon.outer(); + Polygon inverted_polygon; + inverted_polygon.outer().reserve(poly.size()); + std::reverse_copy(poly.begin(), poly.end(), std::back_inserter(inverted_polygon.outer())); + return inverted_polygon; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp new file mode 100644 index 0000000000000..df4ff14dc8e91 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/debug.cpp @@ -0,0 +1,250 @@ +// Copyright 2020 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 +#include +#include + +#include + +#include + +namespace behavior_velocity_planner +{ +namespace +{ +using DebugData = DetectionAreaModule::DebugData; + +lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) +{ + lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0}; + for (const auto & p : poly) { + p_sum += p; + } + return p_sum / poly.size(); +} + +geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) +{ + geometry_msgs::msg::Point msg; + msg.x = point.x(); + msg.y = point.y(); + msg.z = point.z(); + return msg; +} + +visualization_msgs::msg::MarkerArray createMarkerArray( + const DebugData & debug_data, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + const tf2::Transform tf_base_link2front( + tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0)); + + // Stop VirtualWall + const int32_t uid = planning_utils::bitShift(module_id); + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "stop_virtual_wall"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + // DeadLine VirtualWall + for (size_t j = 0; j < debug_data.dead_line_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "dead_line_virtual_wall"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.dead_line_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + // Facto Text + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "factor_text"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "detection area"; + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createCorrespondenceMarkerArray( + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + + const lanelet::ConstLineString3d stop_line = detection_area_reg_elem.stopLine(); + const auto stop_line_center_point = + (stop_line.front().basicPoint() + stop_line.back().basicPoint()) / 2; + + // ID + { + auto marker = createDefaultMarker( + "map", now, "detection_area_id", detection_area_reg_elem.id(), + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.scale = createMarkerScale(0.0, 0.0, 1.0); + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { + const auto poly = detection_area.basicPolygon(); + + marker.pose.position = toMsg(poly.front()); + marker.pose.position.z += 2.0; + marker.text = std::to_string(detection_area_reg_elem.id()); + + msg.markers.push_back(marker); + } + } + + // Polygon + { + auto marker = createDefaultMarker( + "map", now, "detection_area_polygon", detection_area_reg_elem.id(), + visualization_msgs::msg::Marker::LINE_LIST, createMarkerColor(0.1, 0.1, 1.0, 0.500)); + marker.scale = createMarkerScale(0.1, 0.0, 0.0); + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { + const auto poly = detection_area.basicPolygon(); + + for (size_t i = 0; i < poly.size(); ++i) { + const auto idx_front = i; + const auto idx_back = (i == poly.size() - 1) ? 0 : i + 1; + + const auto & p_front = poly.at(idx_front); + const auto & p_back = poly.at(idx_back); + + marker.points.push_back(toMsg(p_front)); + marker.points.push_back(toMsg(p_back)); + } + } + + msg.markers.push_back(marker); + } + + // Polygon to StopLine + { + auto marker = createDefaultMarker( + "map", now, "detection_area_correspondence", detection_area_reg_elem.id(), + visualization_msgs::msg::Marker::LINE_LIST, createMarkerColor(0.1, 0.1, 1.0, 0.500)); + marker.scale = createMarkerScale(0.1, 0.0, 0.0); + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + for (const auto & detection_area : detection_area_reg_elem.detectionAreas()) { + const auto poly = detection_area.basicPolygon(); + const auto centroid_point = getCentroidPoint(poly); + for (size_t i = 0; i < poly.size(); ++i) { + marker.points.push_back(toMsg(centroid_point)); + marker.points.push_back(toMsg(stop_line_center_point)); + } + } + + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createObstacleMarkerArray( + const std::vector & obstacle_points, const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + + { + auto marker = createDefaultMarker( + "map", now, "obstacles", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerColor(1.0, 0.0, 0.0, 0.999)); + marker.scale = createMarkerScale(0.3, 0.3, 0.3); + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + for (size_t i = 0; i < obstacle_points.size(); ++i) { + marker.id = i; + marker.pose.position = obstacle_points.at(i); + + msg.markers.push_back(marker); + } + } + + return msg; +} + +} // namespace + +visualization_msgs::msg::MarkerArray DetectionAreaModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + const rclcpp::Time current_time = clock_->now(); + appendMarkerArray( + createMarkerArray(debug_data_, getModuleId()), current_time, &debug_marker_array); + + if (!debug_data_.stop_poses.empty()) { + appendMarkerArray( + createCorrespondenceMarkerArray(detection_area_reg_elem_, current_time), current_time, + &debug_marker_array); + + appendMarkerArray( + createObstacleMarkerArray(debug_data_.obstacle_points, current_time), current_time, + &debug_marker_array); + } + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp new file mode 100644 index 0000000000000..f07106d6ea1af --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -0,0 +1,97 @@ +// Copyright 2020 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 +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::vector getDetectionAreaRegElemsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector detection_area_reg_elems; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + const auto ll = lanelet_map->laneletLayer.get(lane_id); + const auto detection_areas = ll.regulatoryElementsAs(); + for (const auto & detection_area : detection_areas) { + detection_area_reg_elems.push_back(detection_area); + } + } + + return detection_area_reg_elems; +} + +std::set getDetectionAreaIdSetOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::set detection_area_id_set; + for (const auto & detection_area : getDetectionAreaRegElemsOnPath(path, lanelet_map)) { + detection_area_id_set.insert(detection_area->id()); + } + return detection_area_id_set; +} +} // namespace + +DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0); + planner_param_.use_dead_line = node.declare_parameter(ns + ".use_dead_line", false); + planner_param_.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 5.0); + planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false); + planner_param_.state_clear_time = node.declare_parameter(ns + ".state_clear_time", 2.0); +} + +void DetectionAreaModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & detection_area : + getDetectionAreaRegElemsOnPath(path, planner_data_->lanelet_map)) { + // Use lanelet_id to unregister module when the route is changed + const auto module_id = detection_area->id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, *detection_area, planner_param_, logger_.get_child("detection_area_module"), + clock_)); + } + } +} + +std::function &)> +DetectionAreaModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto detection_area_id_set = getDetectionAreaIdSetOnPath(path, planner_data_->lanelet_map); + + return [detection_area_id_set](const std::shared_ptr & scene_module) { + return detection_area_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp new file mode 100644 index 0000000000000..0f112aeb7f28a --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -0,0 +1,495 @@ +// Copyright 2020 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 +#include + +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +namespace +{ +std::pair findWayPointAndDistance( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & p) +{ + constexpr double max_lateral_dist = 3.0; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + const double dx = p.x - p_front.x; + const double dy = p.y - p_front.y; + const double dx_wp = p_back.x - p_front.x; + const double dy_wp = p_back.y - p_front.y; + + const double theta = std::atan2(dy, dx) - std::atan2(dy_wp, dx_wp); + + const double dist = std::hypot(dx, dy); + const double dist_wp = std::hypot(dx_wp, dy_wp); + + // check lateral distance + if (std::fabs(dist * std::sin(theta)) > max_lateral_dist) { + continue; + } + + // if the point p is back of the way point, return negative distance + if (dist * std::cos(theta) < 0) { + return std::make_pair(static_cast(i), -1.0 * dist); + } + + if (dist * std::cos(theta) < dist_wp) { + return std::make_pair(static_cast(i), dist); + } + } + + // if the way point is not found, return negative distance from the way point at 0 + const double dx = p.x - path.points.front().point.pose.position.x; + const double dy = p.y - path.points.front().point.pose.position.y; + return std::make_pair(-1, -1.0 * std::hypot(dx, dy)); +} + +double calcArcLengthFromWayPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int & src, const int & dst) +{ + double length = 0; + const size_t src_idx = src >= 0 ? static_cast(src) : 0; + const size_t dst_idx = dst >= 0 ? static_cast(dst) : 0; + for (size_t i = src_idx; i < dst_idx; ++i) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + length += std::hypot(p_back.x - p_front.x, p_back.y - p_front.y); + } + return length; +} + +double calcSignedArcLength( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + const std::pair src = findWayPointAndDistance(path, p1); + const std::pair dst = findWayPointAndDistance(path, p2); + if (dst.first == -1) { + const double dx = p1.x - p2.x; + const double dy = p1.y - p2.y; + return -1.0 * std::hypot(dx, dy); + } + + if (src.first < dst.first) { + return calcArcLengthFromWayPoint(path, src.first, dst.first) - src.second + dst.second; + } else if (src.first > dst.first) { + return -1.0 * (calcArcLengthFromWayPoint(path, dst.first, src.first) - dst.second + src.second); + } else { + return dst.second - src.second; + } +} + +double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) +{ + Eigen::Affine3d map2p1; + tf2::fromMsg(p1, map2p1); + const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); + return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); +} + +double calcYawFromPoints( + const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back) +{ + return std::atan2(p_back.y - p_front.y, p_back.x - p_front.x); +} + +boost::optional getNearestCollisionPoint( + const LineString2d & stop_line, const LineString2d & path_segment) +{ + // Find all collision points + std::vector collision_points; + bg::intersection(stop_line, path_segment, collision_points); + if (collision_points.empty()) { + return {}; + } + + // To dist list + std::vector dist_list; + dist_list.reserve(collision_points.size()); + std::transform( + collision_points.cbegin(), collision_points.cend(), std::back_inserter(dist_list), + [&path_segment](const Point2d & collision_point) { + return bg::distance(path_segment.front(), collision_point); + }); + + // Find nearest collision point + const auto min_itr = std::min_element(dist_list.cbegin(), dist_list.cend()); + const auto min_idx = std::distance(dist_list.cbegin(), min_itr); + + return collision_points.at(min_idx); +} + +boost::optional findCollisionSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line) +{ + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point + const auto & p2 = path.points.at(i + 1).point.pose.position; // Point after collision point + + const LineString2d path_segment = {{p1.x, p1.y}, {p2.x, p2.y}}; + + const auto nearest_collision_point = getNearestCollisionPoint(stop_line, path_segment); + if (nearest_collision_point) { + return std::make_pair(i, *nearest_collision_point); + } + } + + return {}; +} + +boost::optional findForwardOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, + const double offset_length) +{ + double sum_length = 0.0; + for (size_t i = base_idx; i < path.points.size() - 1; ++i) { + const auto p_front = to_bg2d(path.points.at(i).point.pose.position); + const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position); + + sum_length += bg::distance(p_front, p_back); + + // If it's over offset point, return front index and remain offset length + if (sum_length >= offset_length) { + return std::make_pair(i, sum_length - offset_length); + } + } + + // No enough path length + return {}; +} + +boost::optional findBackwardOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, + const double offset_length) +{ + double sum_length = 0.0; + const auto start = static_cast(base_idx) - 1; + for (std::int32_t i = start; i >= 0; --i) { + const auto p_front = to_bg2d(path.points.at(i).point.pose.position); + const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position); + + sum_length += bg::distance(p_front, p_back); + + // If it's over offset point, return front index and remain offset length + if (sum_length >= offset_length) { + const auto k = static_cast(i); + return std::make_pair(k, sum_length - offset_length); + } + } + + // No enough path length + return {}; +} + +boost::optional findOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const PathIndexWithPoint2d & collision_segment, const double offset_length) +{ + const size_t & collision_idx = collision_segment.first; + const Point2d & collision_point = collision_segment.second; + const auto p_front = to_bg2d(path.points.at(collision_idx).point.pose.position); + const auto p_back = to_bg2d(path.points.at(collision_idx + 1).point.pose.position); + + if (offset_length >= 0) { + return findForwardOffsetSegment( + path, collision_idx, offset_length + bg::distance(p_front, collision_point)); + } else { + return findBackwardOffsetSegment( + path, collision_idx + 1, -offset_length + bg::distance(p_back, collision_point)); + } +} + +geometry_msgs::msg::Pose calcTargetPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const PathIndexWithOffset & offset_segment) +{ + const size_t offset_idx = offset_segment.first; + const double remain_offset_length = offset_segment.second; + const auto & p_front = path.points.at(offset_idx).point.pose.position; + const auto & p_back = path.points.at(offset_idx + 1).point.pose.position; + + // To Eigen point + const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); + const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); + + // Calculate interpolation ratio + const auto interpolate_ratio = remain_offset_length / (p_eigen_back - p_eigen_front).norm(); + + // Add offset to front point + const auto target_point_2d = p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); + const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); + + // Calculate orientation so that X-axis would be along the trajectory + tf2::Quaternion quat; + quat.setRPY(0, 0, calcYawFromPoints(p_front, p_back)); + + // To Pose + geometry_msgs::msg::Pose target_pose; + target_pose.position.x = target_point_2d.x(); + target_pose.position.y = target_point_2d.y(); + target_pose.position.z = interpolated_z; + target_pose.orientation = tf2::toMsg(quat); + + return target_pose; +} +} // namespace + +DetectionAreaModule::DetectionAreaModule( + const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + detection_area_reg_elem_(detection_area_reg_elem), + state_(State::GO), + planner_param_(planner_param) +{ +} + +LineString2d DetectionAreaModule::getStopLineGeometry2d() const +{ + const lanelet::ConstLineString3d stop_line = detection_area_reg_elem_.stopLine(); + return planning_utils::extendLine( + stop_line[0], stop_line[1], planner_data_->stop_line_extend_length); +} + +bool DetectionAreaModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + // Store original path + const auto original_path = *path; + + // Reset data + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::DETECTION_AREA); + + // Find obstacles in detection area + const auto obstacle_points = getObstaclePoints(); + debug_data_.obstacle_points = obstacle_points; + if (!obstacle_points.empty()) { + last_obstacle_found_time_ = std::make_shared(clock_->now()); + } + + // Check state + if (canClearStopState()) { + state_ = State::GO; + last_obstacle_found_time_ = {}; + return true; + } + + // Get stop line geometry + const auto stop_line = getStopLineGeometry2d(); + + // Get self pose + const auto & self_pose = planner_data_->current_pose.pose; + + // Force ignore objects after dead_line + if (planner_param_.use_dead_line) { + // Use '-' for margin because it's the backward distance from stop line + const auto dead_line_point = + createTargetPoint(original_path, stop_line, -planner_param_.dead_line_margin); + + if (dead_line_point) { + const auto & dead_line_pose = dead_line_point->second; + debug_data_.dead_line_poses.push_back(dead_line_pose); + + if (isOverLine(original_path, self_pose, dead_line_pose)) { + RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); + return true; + } + } + } + + // Get stop point + const auto stop_point = createTargetPoint(original_path, stop_line, planner_param_.stop_margin); + if (!stop_point) { + return true; + } + + const auto & stop_pose = stop_point->second; + + // Ignore objects detected after stop_line if not in STOP state + if (state_ != State::STOP && isOverLine(original_path, self_pose, stop_pose)) { + return true; + } + + // Ignore objects if braking distance is not enough + if (planner_param_.use_pass_judge_line) { + if (state_ != State::STOP && !hasEnoughBrakingDistance(self_pose, stop_pose)) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, std::chrono::milliseconds(1000).count(), + "[detection_area] vehicle is over stop border"); + return true; + } + } + + // Insert stop point + state_ = State::STOP; + *path = insertStopPoint(original_path, *stop_point); + + // For virtual wall + debug_data_.stop_poses.push_back(stop_pose); + + // Create StopReason + { + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_point->second; + stop_factor.stop_factor_points = obstacle_points; + planning_utils::appendStopReason(stop_factor, stop_reason); + } + + // Create legacy StopReason + { + const auto insert_idx = stop_point->first + 1; + + if ( + !first_stop_path_point_index_ || + static_cast(insert_idx) < first_stop_path_point_index_) { + debug_data_.first_stop_pose = stop_pose; + first_stop_path_point_index_ = static_cast(insert_idx); + } + } + + return true; +} + +std::vector DetectionAreaModule::getObstaclePoints() const +{ + std::vector obstacle_points; + + const auto detection_areas = detection_area_reg_elem_.detectionAreas(); + const auto & points = *(planner_data_->no_ground_pointcloud); + + for (const auto & detection_area : detection_areas) { + for (const auto p : points) { + if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(detection_area).basicPolygon())) { + obstacle_points.push_back(planning_utils::toRosPoint(p)); + } + } + } + + return obstacle_points; +} + +bool DetectionAreaModule::canClearStopState() const +{ + // vehicle can clear stop state if the obstacle has never appeared in detection area + if (!last_obstacle_found_time_) { + return true; + } + + // vehicle can clear stop state if the certain time has passed since the obstacle disappeared + const auto elapsed_time = clock_->now() - *last_obstacle_found_time_; + if (elapsed_time.seconds() >= planner_param_.state_clear_time) { + return true; + } + + // rollback in simulation mode + if (elapsed_time.seconds() < 0.0) { + return true; + } + + return false; +} + +bool DetectionAreaModule::isOverLine( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const +{ + return calcSignedArcLength(path, self_pose.position, line_pose.position) < 0; +} + +bool DetectionAreaModule::hasEnoughBrakingDistance( + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const +{ + // get vehicle info and compute pass_judge_line_distance + const auto current_velocity = planner_data_->current_velocity->twist.linear.x; + const double max_acc = planner_data_->max_stop_acceleration_threshold; + const double delay_response_time = planner_data_->delay_response_time; + const double pass_judge_line_distance = + planning_utils::calcJudgeLineDistWithAccLimit(current_velocity, max_acc, delay_response_time); + + return calcSignedDistance(self_pose, line_pose.position) > pass_judge_line_distance; +} + +autoware_auto_planning_msgs::msg::PathWithLaneId DetectionAreaModule::insertStopPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const PathIndexWithPose & stop_point) const +{ + auto output_path = path; + + const auto insert_idx = stop_point.first + 1; + const auto stop_pose = stop_point.second; + + // To PathPointWithLaneId + autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + stop_point_with_lane_id = output_path.points.at(insert_idx); + stop_point_with_lane_id.point.pose = stop_pose; + stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + + // Insert stop point + output_path.points.insert(output_path.points.begin() + insert_idx, stop_point_with_lane_id); + + // Insert 0 velocity after stop point + for (size_t j = insert_idx; j < output_path.points.size(); ++j) { + output_path.points.at(j).point.longitudinal_velocity_mps = 0.0; + } + + return output_path; +} + +boost::optional DetectionAreaModule::createTargetPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin) const +{ + // Find collision segment + const auto collision_segment = findCollisionSegment(path, stop_line); + if (!collision_segment) { + // No collision + return {}; + } + + // Calculate offset length from stop line + // Use '-' to make the positive direction is forward + const double offset_length = -(margin + planner_data_->vehicle_info_.max_longitudinal_offset_m); + + // Find offset segment + const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length); + if (!offset_segment) { + // No enough path length + return {}; + } + + const auto front_idx = offset_segment->first; + const auto target_pose = calcTargetPose(path, *offset_segment); + + return std::make_pair(front_idx, target_pose); +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp new file mode 100644 index 0000000000000..d346856e8eadc --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/debug.cpp @@ -0,0 +1,354 @@ +// Copyright 2020 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 +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +using State = IntersectionModule::State; + +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( + const std::vector & polygons, const std::string & ns, + const int64_t lane_id) +{ + visualization_msgs::msg::MarkerArray msg; + + int32_t i = 0; + int32_t uid = planning_utils::bitShift(lane_id); + for (const auto & polygon : polygons) { + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + + marker.ns = ns; + marker.id = uid + i++; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = createMarkerScale(0.1, 0.0, 0.0); + marker.color = createMarkerColor(0.0, 1.0, 0.0, 0.999); + for (const auto & p : polygon) { + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + marker.points.push_back(point); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createPolygonMarkerArray( + const geometry_msgs::msg::Polygon & polygon, const std::string & ns, const int64_t lane_id, + const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + + marker.ns = ns; + marker.id = lane_id; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker.scale = createMarkerScale(0.3, 0.0, 0.0); + marker.color = createMarkerColor(r, g, b, 0.8); + for (const auto & p : polygon.points) { + geometry_msgs::msg::Point point; + point.x = p.x; + point.y = p.y; + point.z = p.z; + marker.points.push_back(point); + } + if (!marker.points.empty()) { + marker.points.push_back(marker.points.front()); + } + msg.markers.push_back(marker); + + return msg; +} + +visualization_msgs::msg::MarkerArray createObjectsMarkerArray( + const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const int64_t lane_id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = ns; + + int32_t uid = planning_utils::bitShift(lane_id); + int32_t i = 0; + for (const auto & object : objects.objects) { + marker.id = uid + i++; + marker.lifetime = rclcpp::Duration::from_seconds(1.0); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = object.kinematics.initial_pose_with_covariance.pose; + marker.scale = createMarkerScale(3.0, 1.0, 1.0); + marker.color = createMarkerColor(r, g, b, 0.8); + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createPathMarkerArray( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const int64_t lane_id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + int32_t uid = planning_utils::bitShift(lane_id); + int32_t i = 0; + for (const auto & p : path.points) { + visualization_msgs::msg::Marker marker{}; + marker.header.frame_id = "map"; + marker.ns = ns; + marker.id = uid + i++; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = p.point.pose; + marker.scale = createMarkerScale(0.6, 0.3, 0.3); + if (std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end()) { + // if p.lane_ids has lane_id + marker.color = createMarkerColor(r, g, b, 0.999); + } else { + marker.color = createMarkerColor(0.5, 0.5, 0.5, 0.999); + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createVirtualStopWallMarkerArray( + const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & stop_factor) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker_virtual_wall{}; + marker_virtual_wall.header.frame_id = "map"; + marker_virtual_wall.ns = "stop_virtual_wall"; + marker_virtual_wall.id = lane_id; + marker_virtual_wall.lifetime = rclcpp::Duration::from_seconds(0.5); + marker_virtual_wall.type = visualization_msgs::msg::Marker::CUBE; + marker_virtual_wall.action = visualization_msgs::msg::Marker::ADD; + marker_virtual_wall.pose = pose; + marker_virtual_wall.pose.position.z += 1.0; + marker_virtual_wall.scale = createMarkerScale(0.1, 5.0, 2.0); + marker_virtual_wall.color = createMarkerColor(1.0, 0.0, 0.0, 0.5); + msg.markers.push_back(marker_virtual_wall); + + visualization_msgs::msg::Marker marker_factor_text{}; + marker_factor_text.header.frame_id = "map"; + marker_factor_text.ns = "stop_factor_text"; + marker_factor_text.id = lane_id; + marker_factor_text.lifetime = rclcpp::Duration::from_seconds(0.5); + marker_factor_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_factor_text.action = visualization_msgs::msg::Marker::ADD; + marker_factor_text.pose = pose; + marker_factor_text.pose.position.z += 2.0; + marker_factor_text.scale = createMarkerScale(0.0, 0.0, 1.0); + marker_factor_text.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); + marker_factor_text.text = stop_factor; + msg.markers.push_back(marker_factor_text); + + return msg; +} + +visualization_msgs::msg::MarkerArray createVirtualSlowWallMarkerArray( + const geometry_msgs::msg::Pose & pose, const int64_t lane_id, const std::string & slow_factor) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker_virtual_wall{}; + marker_virtual_wall.header.frame_id = "map"; + marker_virtual_wall.ns = "slow_virtual_wall"; + marker_virtual_wall.id = lane_id; + marker_virtual_wall.lifetime = rclcpp::Duration::from_seconds(0.5); + marker_virtual_wall.type = visualization_msgs::msg::Marker::CUBE; + marker_virtual_wall.action = visualization_msgs::msg::Marker::ADD; + marker_virtual_wall.pose = pose; + marker_virtual_wall.pose.position.z += 1.0; + marker_virtual_wall.scale = createMarkerScale(0.1, 5.0, 2.0); + marker_virtual_wall.color = createMarkerColor(1.0, 1.0, 0.0, 0.5); + msg.markers.push_back(marker_virtual_wall); + + visualization_msgs::msg::Marker marker_factor_text{}; + marker_factor_text.header.frame_id = "map"; + marker_factor_text.ns = "slow_factor_text"; + marker_factor_text.id = lane_id; + marker_factor_text.lifetime = rclcpp::Duration::from_seconds(0.5); + marker_factor_text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_factor_text.action = visualization_msgs::msg::Marker::ADD; + marker_factor_text.pose = pose; + marker_factor_text.pose.position.z += 2.0; + marker_factor_text.scale = createMarkerScale(0.0, 0.0, 1.0); + marker_factor_text.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); + marker_factor_text.text = slow_factor; + msg.markers.push_back(marker_factor_text); + + return msg; +} + +visualization_msgs::msg::MarkerArray createPoseMarkerArray( + const geometry_msgs::msg::Pose & pose, const std::string & ns, const int64_t id, const double r, + const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker_line{}; + marker_line.header.frame_id = "map"; + marker_line.ns = ns + "_line"; + marker_line.id = id; + marker_line.lifetime = rclcpp::Duration::from_seconds(0.3); + marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker_line.action = visualization_msgs::msg::Marker::ADD; + marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); + marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); + marker_line.color = createMarkerColor(r, g, b, 0.999); + + const double yaw = tf2::getYaw(pose.orientation); + + const double a = 3.0; + geometry_msgs::msg::Point p0; + p0.x = pose.position.x - a * std::sin(yaw); + p0.y = pose.position.y + a * std::cos(yaw); + p0.z = pose.position.z; + marker_line.points.push_back(p0); + + geometry_msgs::msg::Point p1; + p1.x = pose.position.x + a * std::sin(yaw); + p1.y = pose.position.y - a * std::cos(yaw); + p1.z = pose.position.z; + marker_line.points.push_back(p1); + + msg.markers.push_back(marker_line); + + return msg; +} + +} // namespace + +visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + const auto state = state_machine_.getState(); + const auto current_time = this->clock_->now(); + + appendMarkerArray( + createPathMarkerArray(debug_data_.path_raw, "path_raw", lane_id_, 0.0, 1.0, 1.0), current_time, + &debug_marker_array); + + appendMarkerArray( + createLaneletPolygonsMarkerArray(debug_data_.detection_area, "detection_area", lane_id_), + current_time, &debug_marker_array); + + appendMarkerArray( + createPolygonMarkerArray(debug_data_.ego_lane_polygon, "ego_lane", lane_id_, 0.0, 0.3, 0.7), + current_time, &debug_marker_array); + + appendMarkerArray( + createPolygonMarkerArray( + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", lane_id_, 0.0, 0.5, 0.5), + current_time, &debug_marker_array); + + appendMarkerArray( + createPolygonMarkerArray( + debug_data_.candidate_collision_ego_lane_polygon, "candidate_collision_ego_lane_polygon", + lane_id_, 0.5, 0.0, 0.0), + current_time, &debug_marker_array); + + size_t i{0}; + for (const auto & p : debug_data_.candidate_collision_object_polygons) { + appendMarkerArray( + createPolygonMarkerArray( + p, "candidate_collision_object_polygons", lane_id_ + i++, 0.0, 0.5, 0.5), + current_time, &debug_marker_array); + } + + appendMarkerArray( + createObjectsMarkerArray( + debug_data_.conflicting_targets, "conflicting_targets", lane_id_, 0.99, 0.4, 0.0), + current_time, &debug_marker_array); + + appendMarkerArray( + createObjectsMarkerArray(debug_data_.stuck_targets, "stuck_targets", lane_id_, 0.99, 0.99, 0.2), + current_time, &debug_marker_array); + + if (state == IntersectionModule::State::STOP) { + appendMarkerArray( + createPoseMarkerArray( + debug_data_.stop_point_pose, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), + current_time, &debug_marker_array); + + appendMarkerArray( + createPoseMarkerArray( + debug_data_.judge_point_pose, "judge_point_pose", lane_id_, 1.0, 1.0, 0.5), + current_time, &debug_marker_array); + + if (debug_data_.stop_required) { + appendMarkerArray( + createVirtualStopWallMarkerArray(debug_data_.stop_wall_pose, lane_id_, "intersection"), + current_time, &debug_marker_array); + } else { + appendMarkerArray( + createVirtualSlowWallMarkerArray(debug_data_.slow_wall_pose, lane_id_, "intersection"), + current_time, &debug_marker_array); + } + } + + return debug_marker_array; +} + +visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + const auto state = state_machine_.getState(); + + const auto current_time = this->clock_->now(); + if (state == MergeFromPrivateRoadModule::State::STOP) { + appendMarkerArray( + createPoseMarkerArray( + debug_data_.stop_point_pose, "stop_point_pose", lane_id_, 1.0, 0.0, 0.0), + current_time, &debug_marker_array); + + appendMarkerArray( + createVirtualStopWallMarkerArray( + debug_data_.virtual_wall_pose, lane_id_, "merge_from_private_road"), + current_time, &debug_marker_array); + } + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp new file mode 100644 index 0000000000000..0d235d339af54 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -0,0 +1,141 @@ +// Copyright 2020 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 +#include +#include + +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::vector getLaneletsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector lanelets; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + const auto lane = lanelet_map->laneletLayer.get(lane_id); + if (!lanelet::utils::contains(lanelets, lane)) { + lanelets.push_back(lane); + } + } + + return lanelets; +} + +std::set getLaneIdSetOnPath(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + std::set lane_id_set; + + for (const auto & p : path.points) { + for (const auto & lane_id : p.lane_ids) { + lane_id_set.insert(lane_id); + } + } + + return lane_id_set; +} + +} // namespace + +IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + auto & ip = intersection_param_; + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); + ip.decel_velocity = node.declare_parameter(ns + ".decel_velocity", 30.0 / 3.6); + ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); + ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + + vehicle_info.max_longitudinal_offset_m; + ip.stuck_vehicle_vel_thr = node.declare_parameter(ns + ".stuck_vehicle_vel_thr", 3.0 / 3.6); + ip.intersection_velocity = node.declare_parameter(ns + ".intersection_velocity", 10.0 / 3.6); + ip.intersection_max_acc = node.declare_parameter(ns + ".intersection_max_accel", 0.5); + ip.detection_area_margin = node.declare_parameter(ns + ".detection_area_margin", 0.5); + ip.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0); + ip.detection_area_angle_thr = + node.declare_parameter(ns + ".detection_area_angle_threshold", M_PI / 4.0); + ip.min_predicted_path_confidence = + node.declare_parameter(ns + ".min_predicted_path_confidence", 0.05); + ip.external_input_timeout = node.declare_parameter(ns + ".walkway.external_input_timeout", 1.0); + ip.collision_start_margin_time = node.declare_parameter(ns + ".collision_start_margin_time", 5.0); + ip.collision_end_margin_time = node.declare_parameter(ns + ".collision_end_margin_time", 2.0); + auto & mp = merge_from_private_area_param_; + mp.stop_duration_sec = + node.declare_parameter(ns + ".merge_from_private_area.stop_duration_sec", 1.0); + mp.intersection_param = intersection_param_; +} + +void IntersectionModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lanelets = getLaneletsOnPath(path, planner_data_->lanelet_map); + for (size_t i = 0; i < lanelets.size(); i++) { + const auto ll = lanelets.at(i); + const auto lane_id = ll.id(); + const auto module_id = lane_id; + + if (isModuleRegistered(module_id)) { + continue; + } + + // Is intersection? + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + // Is merging from private road? + if (i + 1 < lanelets.size()) { + const auto next_lane = lanelets.at(i + 1); + const std::string lane_location = ll.attributeOr("location", "else"); + const std::string next_lane_location = next_lane.attributeOr("location", "else"); + if (lane_location == "private" && next_lane_location != "private") { + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, + logger_.get_child("merge_from_private_road_module"), clock_)); + } + } + + registerModule(std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, + logger_.get_child("intersection_module"), clock_)); + } +} + +std::function &)> +IntersectionModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_id_set = getLaneIdSetOnPath(path); + + return [lane_id_set](const std::shared_ptr & scene_module) { + return lane_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp new file mode 100644 index 0000000000000..c6df86aa2ad22 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -0,0 +1,666 @@ +// Copyright 2020 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +static geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( + const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) +{ + if (obj_state.initial_twist_with_covariance.twist.linear.x >= 0) { + return obj_state.initial_pose_with_covariance.pose; + } + + // When the object velocity is negative, invert orientation (yaw) + auto obj_pose = obj_state.initial_pose_with_covariance.pose; + double yaw, pitch, roll; + tf2::getEulerYPR(obj_pose.orientation, yaw, pitch, roll); + tf2::Quaternion inv_q; + inv_q.setRPY(roll, pitch, -yaw); + obj_pose.orientation = tf2::toMsg(inv_q); + return obj_pose; +} + +IntersectionModule::IntersectionModule( + const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) +{ + planner_param_ = planner_param; + const auto & assigned_lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); + has_traffic_light_ = + !(assigned_lanelet.regulatoryElementsAs().empty()); + state_machine_.setMarginTime(planner_param_.state_transit_margin_time); +} + +bool IntersectionModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + const bool external_go = + isTargetExternalInputStatus(autoware_api_msgs::msg::IntersectionStatus::GO); + const bool external_stop = + isTargetExternalInputStatus(autoware_api_msgs::msg::IntersectionStatus::STOP); + RCLCPP_DEBUG(logger_, "===== plan start ====="); + debug_data_ = DebugData(); + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::INTERSECTION); + + const auto input_path = *path; + debug_data_.path_raw = input_path; + + State current_state = state_machine_.getState(); + RCLCPP_DEBUG(logger_, "lane_id = %ld, state = %s", lane_id_, toString(current_state).c_str()); + + /* get current pose */ + geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; + + /* get lanelet map */ + const auto lanelet_map_ptr = planner_data_->lanelet_map; + const auto routing_graph_ptr = planner_data_->routing_graph; + + /* get detection area and conflicting area */ + std::vector detection_area_lanelets; + std::vector conflicting_area_lanelets; + + util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_, &conflicting_area_lanelets, + &detection_area_lanelets, logger_); + std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( + conflicting_area_lanelets, planner_param_.detection_area_length); + std::vector detection_areas = util::getPolygon3dFromLaneletsVec( + detection_area_lanelets, planner_param_.detection_area_length); + std::vector conflicting_area_lanelet_ids = + util::getLaneletIdsFromLaneletsVec(conflicting_area_lanelets); + std::vector detection_area_lanelet_ids = + util::getLaneletIdsFromLaneletsVec(detection_area_lanelets); + + if (detection_areas.empty()) { + RCLCPP_DEBUG(logger_, "no detection area. skip computation."); + return true; + } + debug_data_.detection_area = detection_areas; + + /* set stop-line and stop-judgement-line for base_link */ + int stop_line_idx = -1; + int pass_judge_line_idx = -1; + int first_idx_inside_lane = -1; + if (!util::generateStopLine( + lane_id_, conflicting_areas, planner_data_, planner_param_, path, *path, &stop_line_idx, + &pass_judge_line_idx, &first_idx_inside_lane, logger_.get_child("util"))) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return false; + } + + if (stop_line_idx <= 0 || pass_judge_line_idx <= 0) { + RCLCPP_DEBUG(logger_, "stop line or pass judge line is at path[0], ignore planning."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; + } + + /* calc closest index */ + int closest_idx = -1; + if (!planning_utils::calcClosestIndex(input_path, current_pose.pose, closest_idx)) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "calcClosestIndex fail"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return false; + } + + /* if current_state = GO, and current_pose is in front of stop_line, ignore planning. */ + bool is_over_pass_judge_line = static_cast(closest_idx > pass_judge_line_idx); + if (closest_idx == pass_judge_line_idx) { + geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose; + is_over_pass_judge_line = util::isAheadOf(current_pose.pose, pass_judge_line); + } + if (current_state == State::GO && is_over_pass_judge_line && !external_stop) { + RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; // no plan needed. + } + + /* get dynamic object */ + const auto objects_ptr = planner_data_->predicted_objects; + + /* calculate dynamic collision around detection area */ + bool has_collision = checkCollision( + lanelet_map_ptr, *path, detection_areas, detection_area_lanelet_ids, objects_ptr, closest_idx); + bool is_stuck = checkStuckVehicleInIntersection( + lanelet_map_ptr, *path, closest_idx, stop_line_idx, objects_ptr); + bool is_entry_prohibited = (has_collision || is_stuck); + if (external_go) { + is_entry_prohibited = false; + } else if (external_stop) { + is_entry_prohibited = true; + } + state_machine_.setStateWithMarginTime( + is_entry_prohibited ? State::STOP : State::GO, logger_.get_child("state_machine"), *clock_); + + /* set stop speed : TODO behavior on straight lane should be improved*/ + if (state_machine_.getState() == State::STOP) { + constexpr double stop_vel = 0.0; + const double decel_vel = planner_param_.decel_velocity; + const bool is_stop_required = is_stuck || !has_traffic_light_ || turn_direction_ != "straight"; + const double v = is_stop_required ? stop_vel : decel_vel; + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + util::setVelocityFrom(stop_line_idx, v, path); + + if (is_stop_required) { + debug_data_.stop_required = true; + debug_data_.stop_wall_pose = util::getAheadPose(stop_line_idx, base_link2front, *path); + debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; + debug_data_.judge_point_pose = path->points.at(pass_judge_line_idx).point.pose; + + /* get stop point and stop factor */ + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.stop_point_pose; + const auto stop_factor_conflict = + planning_utils::toRosPoints(debug_data_.conflicting_targets); + const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); + stop_factor.stop_factor_points = + planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); + planning_utils::appendStopReason(stop_factor, stop_reason); + + } else { + debug_data_.stop_required = false; + debug_data_.slow_wall_pose = util::getAheadPose(stop_line_idx, base_link2front, *path); + } + } + + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; +} + +void IntersectionModule::cutPredictPathWithDuration( + autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const +{ + const rclcpp::Time current_time = clock_->now(); + for (auto & object : objects_ptr->objects) { // each objects + for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + const auto origin_path = predicted_path; + predicted_path.path.clear(); + + for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points + const auto & predicted_pose = origin_path.path.at(k); + const auto predicted_time = + rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Duration(origin_path.time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + predicted_path.path.push_back(predicted_pose); + } + } + } + } +} + +bool IntersectionModule::checkCollision( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::vector & detection_areas, + const std::vector & detection_area_lanelet_ids, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const int closest_idx) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getPolygonFromArcLength; + + /* generate ego-lane polygon */ + const auto ego_poly = + generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0); + + debug_data_.ego_lane_polygon = toGeomMsg(ego_poly); + + /* extract target objects */ + autoware_auto_perception_msgs::msg::PredictedObjects target_objects; + target_objects.header = objects_ptr->header; + for (const auto & object : objects_ptr->objects) { + // ignore non-vehicle type objects, such as pedestrian. + if (!isTargetCollisionVehicleType(object)) { + continue; + } + + // ignore vehicle in ego-lane. (TODO update check algorithm) + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const bool is_in_ego_lane = bg::within(to_bg2d(object_pose.position), ego_poly); + if (is_in_ego_lane) { + continue; // TODO(Kenji Miyake): check direction? + } + + // keep vehicle in detection_area + const Point2d obj_point( + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y); + for (const auto & detection_area : detection_areas) { + const auto detection_poly = lanelet::utils::to2D(detection_area).basicPolygon(); + const double dist_to_detection_area = + boost::geometry::distance(obj_point, toBoostPoly(detection_poly)); + if (dist_to_detection_area > planner_param_.detection_area_margin) { + // ignore the object far from detection area + continue; + } + // check direction of objects + const auto object_direction = getObjectPoseWithVelocityDirection(object.kinematics); + if (checkAngleForTargetLanelets(object_direction, detection_area_lanelet_ids)) { + target_objects.objects.push_back(object); + break; + } + } + } + + /* check collision between target_objects predicted path and ego lane */ + + // cut the predicted path at passing_time + const auto time_distance_array = calcIntersectionPassingTime(path, closest_idx, lane_id_); + const double passing_time = time_distance_array.back().first; + cutPredictPathWithDuration(&target_objects, passing_time); + + lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); + const auto closest_arc_coords = getArcCoordinates( + ego_lane_with_next_lane, autoware_utils::getPose(path.points.at(closest_idx).point)); + const double distance_until_intersection = + calcDistanceUntilIntersectionLanelet(lanelet_map_ptr, path, closest_idx); + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + // check collision between predicted_path and ego_area + bool collision_detected = false; + for (const auto & object : target_objects.objects) { + bool has_collision = false; + for (const auto & predicted_path : object.kinematics.predicted_paths) { + if (predicted_path.confidence < planner_param_.min_predicted_path_confidence) { + // ignore the predicted path with too low confidence + continue; + } + + std::vector predicted_poses; + for (const auto & pose : predicted_path.path) { + predicted_poses.push_back(pose); + } + has_collision = bg::intersects(ego_poly, to_bg2d(predicted_poses)); + if (has_collision) { + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), + [&ego_poly](const auto & a, const auto & b) { + return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + }); + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&ego_poly](const auto & a, const auto & b) { + return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + }); + const double ref_object_enter_time = + static_cast(first_itr - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto start_time_distance_itr = time_distance_array.begin(); + if (ref_object_enter_time - planner_param_.collision_start_margin_time > 0) { + start_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_enter_time - planner_param_.collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (start_time_distance_itr == time_distance_array.end()) { + continue; + } + } + const double ref_object_exit_time = + static_cast(last_itr.base() - predicted_path.path.begin()) * + rclcpp::Duration(predicted_path.time_step).seconds(); + auto end_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_exit_time + planner_param_.collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (end_time_distance_itr == time_distance_array.end()) { + end_time_distance_itr = time_distance_array.end() - 1; + } + const double start_arc_length = std::max( + 0.0, closest_arc_coords.length + (*start_time_distance_itr).second - + distance_until_intersection); + const double end_arc_length = std::max( + 0.0, closest_arc_coords.length + (*end_time_distance_itr).second + base_link2front - + distance_until_intersection); + const auto trimmed_ego_polygon = + getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length); + + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + + polygon.outer().emplace_back(polygon.outer().front()); + + debug_data_.candidate_collision_ego_lane_polygon = toGeomMsg(polygon); + + for (auto itr = first_itr; itr != last_itr.base(); ++itr) { + const auto footprint_polygon = toPredictedFootprintPolygon(object, *itr); + debug_data_.candidate_collision_object_polygons.emplace_back( + toGeomMsg(footprint_polygon)); + if (bg::intersects(polygon, footprint_polygon)) { + collision_detected = true; + break; + } + } + if (collision_detected) { + debug_data_.conflicting_targets.objects.push_back(object); + break; + } + } + } + } + + return collision_detected; +} + +Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const int start_idx, const double extra_dist, const double ignore_dist) const +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + + lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); + + const auto start_arc_coords = getArcCoordinates( + ego_lane_with_next_lane, autoware_utils::getPose(path.points.at(start_idx).point)); + + const auto closest_arc_coords = getArcCoordinates( + ego_lane_with_next_lane, autoware_utils::getPose(path.points.at(closest_idx).point)); + + const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length + ? closest_arc_coords.length + : start_arc_coords.length + ignore_dist; + + const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; + + const auto target_polygon = + to2D(getPolygonFromArcLength(ego_lane_with_next_lane, start_arc_length, end_arc_length)) + .basicPolygon(); + + Polygon2d polygon{}; + + if (target_polygon.empty()) { + return polygon; + } + + for (const auto & p : target_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + + polygon.outer().emplace_back(polygon.outer().front()); + + return polygon; +} + +TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const int objective_lane_id) const +{ + TimeDistanceArray time_distance_array{}; + double closest_vel = + (std::max(1e-01, std::fabs(planner_data_->current_velocity->twist.linear.x))); + double dist_sum = 0.0; + double passing_time = 0.0; + time_distance_array.emplace_back(passing_time, dist_sum); + int assigned_lane_found = false; + + for (size_t i = closest_idx + 1; i < path.points.size(); ++i) { + const double dist = planning_utils::calcDist2d(path.points.at(i - 1), path.points.at(i)); + dist_sum += dist; + // calc vel in idx i+1 (v_{i+1}^2 - v_{i}^2 = 2ax) + const double next_vel = std::min( + std::sqrt(std::pow(closest_vel, 2.0) + 2.0 * planner_param_.intersection_max_acc * dist), + planner_param_.intersection_velocity); + // calc average vel in idx i~i+1 + const double average_vel = + std::min((closest_vel + next_vel) / 2.0, planner_param_.intersection_velocity); + passing_time += dist / average_vel; + time_distance_array.emplace_back(passing_time, dist_sum); + closest_vel = next_vel; + + bool has_objective_lane_id = util::hasLaneId(path.points.at(i), objective_lane_id); + + if (assigned_lane_found && !has_objective_lane_id) { + break; + } + assigned_lane_found = has_objective_lane_id; + } + if (!assigned_lane_found) { + return {{0.0, 0.0}}; // has already passed the intersection. + } + + RCLCPP_DEBUG(logger_, "intersection dist = %f, passing_time = %f", dist_sum, passing_time); + + return time_distance_array; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, + const int stop_idx, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr) const +{ + const double detect_length = + planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; + const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon( + lanelet_map_ptr, path, closest_idx, stop_idx, detect_length, + planner_param_.stuck_vehicle_ignore_dist); + debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area); + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); + if (obj_v > planner_param_.stuck_vehicle_vel_thr) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const Polygon2d obj_footprint = toFootprintPolygon(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area) { + RCLCPP_DEBUG(logger_, "stuck vehicle found."); + debug_data_.stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + +Polygon2d IntersectionModule::toFootprintPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + Polygon2d obj_footprint; + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + obj_footprint = toBoostPoly(object.shape.footprint); + } else { + // cylinder type is treated as square-polygon + obj_footprint = + obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); + } + return obj_footprint; +} + +Polygon2d IntersectionModule::toPredictedFootprintPolygon( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + const geometry_msgs::msg::Pose & predicted_pose) const +{ + return obj2polygon(predicted_pose, object.shape.dimensions); +} + +bool IntersectionModule::isTargetCollisionVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { + return true; + } + return false; +} + +bool IntersectionModule::isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} +void IntersectionModule::StateMachine::setStateWithMarginTime( + State state, rclcpp::Logger logger, rclcpp::Clock & clock) +{ + /* same state request */ + if (state_ == state) { + start_time_ = nullptr; // reset timer + return; + } + + /* GO -> STOP */ + if (state == State::STOP) { + state_ = State::STOP; + start_time_ = nullptr; // reset timer + return; + } + + /* STOP -> GO */ + if (state == State::GO) { + if (start_time_ == nullptr) { + start_time_ = std::make_shared(clock.now()); + } else { + const double duration = (clock.now() - *start_time_).seconds(); + if (duration > margin_time_) { + state_ = State::GO; + start_time_ = nullptr; // reset timer + } + } + return; + } + + RCLCPP_ERROR(logger, "Unsuitable state. ignore request."); +} + +void IntersectionModule::StateMachine::setState(State state) { state_ = state; } + +void IntersectionModule::StateMachine::setMarginTime(const double t) { margin_time_ = t; } + +IntersectionModule::State IntersectionModule::StateMachine::getState() { return state_; } + +bool IntersectionModule::isTargetExternalInputStatus(const int target_status) +{ + return planner_data_->external_intersection_status_input && + planner_data_->external_intersection_status_input.get().status == target_status && + (clock_->now() - planner_data_->external_intersection_status_input.get().header.stamp) + .seconds() < planner_param_.external_input_timeout; +} + +bool IntersectionModule::checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const std::vector & target_lanelet_ids) +{ + for (const int lanelet_id : target_lanelet_ids) { + const auto ll = planner_data_->lanelet_map->laneletLayer.get(lanelet_id); + if (!lanelet::utils::isInLanelet(pose, ll, planner_param_.detection_area_margin)) { + continue; + } + const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); + const double pose_angle = tf2::getYaw(pose.orientation); + const double angle_diff = autoware_utils::normalizeRadian(ll_angle - pose_angle); + if (std::fabs(angle_diff) < planner_param_.detection_area_angle_thr) { + return true; + } + } + return false; +} + +lanelet::ConstLanelets IntersectionModule::getEgoLaneWithNextLane( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const +{ + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto last_itr = std::find_if( + path.points.crbegin(), path.points.crend(), + [this](const auto & p) { return p.lane_ids.front() == lane_id_; }); + lanelet::ConstLanelets ego_lane_with_next_lane; + if (last_itr.base() != path.points.end()) { + const auto & next_lanelet = + lanelet_map_ptr->laneletLayer.get((*last_itr.base()).lane_ids.front()); + ego_lane_with_next_lane = {assigned_lanelet, next_lanelet}; + } else { + ego_lane_with_next_lane = {assigned_lanelet}; + } + return ego_lane_with_next_lane; +} + +double IntersectionModule::calcDistanceUntilIntersectionLanelet( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) const +{ + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto intersection_first_itr = std::find_if( + path.points.cbegin(), path.points.cend(), + [this](const auto & p) { return p.lane_ids.front() == lane_id_; }); + if ( + intersection_first_itr == path.points.begin() || intersection_first_itr == path.points.end()) { + return 0.0; + } + const auto dst_idx = std::distance(path.points.begin(), intersection_first_itr) - 1; + + if (closest_idx > static_cast(dst_idx)) { + return 0.0; + } + + double distance = util::calcArcLengthFromPath(path, closest_idx, dst_idx); + const auto & lane_first_point = assigned_lanelet.centerline2d().front(); + distance += std::hypot( + path.points.at(dst_idx).point.pose.position.x - lane_first_point.x(), + path.points.at(dst_idx).point.pose.position.y - lane_first_point.y()); + return distance; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp new file mode 100644 index 0000000000000..fb4671c9d5277 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -0,0 +1,188 @@ +// Copyright 2020 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( + const int64_t module_id, const int64_t lane_id, + [[maybe_unused]] std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id) +{ + planner_param_ = planner_param; + state_machine_.setState(State::STOP); +} + +bool MergeFromPrivateRoadModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = planning_utils::initializeStopReason( + autoware_planning_msgs::msg::StopReason::MERGE_FROM_PRIVATE_ROAD); + + const auto input_path = *path; + debug_data_.path_raw = input_path; + + State current_state = state_machine_.getState(); + RCLCPP_DEBUG(logger_, "lane_id = %ld, state = %s", lane_id_, toString(current_state).c_str()); + + /* get current pose */ + geometry_msgs::msg::PoseStamped current_pose = planner_data_->current_pose; + + /* get lanelet map */ + const auto lanelet_map_ptr = planner_data_->lanelet_map; + const auto routing_graph_ptr = planner_data_->routing_graph; + + /* get detection area */ + std::vector detection_area_lanelets; + std::vector conflicting_area_lanelets; + + util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.intersection_param, + &conflicting_area_lanelets, &detection_area_lanelets, logger_); + std::vector conflicting_areas = util::getPolygon3dFromLaneletsVec( + conflicting_area_lanelets, planner_param_.intersection_param.detection_area_length); + if (conflicting_areas.empty()) { + RCLCPP_DEBUG(logger_, "no detection area. skip computation."); + return true; + } + debug_data_.detection_area = conflicting_areas; + + /* set stop-line and stop-judgement-line for base_link */ + int stop_line_idx = -1; + int judge_line_idx = -1; + int first_idx_inside_lane = -1; + const auto private_path = + extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); + if (!util::generateStopLine( + lane_id_, conflicting_areas, planner_data_, planner_param_.intersection_param, path, + private_path, &stop_line_idx, &judge_line_idx, &first_idx_inside_lane, + logger_.get_child("util"))) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); + return false; + } + + if (stop_line_idx <= 0 || judge_line_idx <= 0) { + RCLCPP_DEBUG(logger_, "stop line or judge line is at path[0], ignore planning."); + return true; + } + + debug_data_.virtual_wall_pose = util::getAheadPose( + stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; + if (first_idx_inside_lane != -1) { + debug_data_.first_collision_point = path->points.at(first_idx_inside_lane).point.pose.position; + } + + /* set stop speed */ + if (state_machine_.getState() == State::STOP) { + constexpr double stop_vel = 0.0; + const double decel_vel = planner_param_.intersection_param.decel_velocity; + double v = (has_traffic_light_ && turn_direction_ == "straight") ? decel_vel : stop_vel; + util::setVelocityFrom(stop_line_idx, v, path); + + /* get stop point and stop factor */ + if (v == stop_vel) { + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.stop_point_pose; + stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); + planning_utils::appendStopReason(stop_factor, stop_reason); + } + + const double distance = + planning_utils::calcDist2d(current_pose.pose, path->points.at(stop_line_idx).point.pose); + constexpr double distance_threshold = 2.0; + if ( + distance < distance_threshold && + planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { + state_machine_.setState(State::GO); + } + + return true; + } + + return true; +} + +autoware_auto_planning_msgs::msg::PathWithLaneId +MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length) +{ + if (path.points.size() < 2) { + return path; + } + + autoware_auto_planning_msgs::msg::PathWithLaneId private_path = path; + private_path.points.clear(); + + double sum_dist = 0.0; + bool prev_has_target_lane_id = false; + for (int i = static_cast(path.points.size()) - 2; i >= 0; i--) { + bool has_target_lane_id = false; + for (const auto path_id : path.points.at(i).lane_ids) { + if (path_id == lane_id_) { + has_target_lane_id = true; + } + } + if (has_target_lane_id) { + // add path point with target lane id + // (lanelet with target lane id is exit of private road) + private_path.points.emplace_back(path.points.at(i)); + prev_has_target_lane_id = true; + continue; + } + if (prev_has_target_lane_id) { + // extend path to the front + private_path.points.emplace_back(path.points.at(i)); + sum_dist += autoware_utils::calcDistance2d( + path.points.at(i).point.pose, path.points.at(i + 1).point.pose); + if (sum_dist > extend_length) { + break; + } + } + } + + std::reverse(private_path.points.begin(), private_path.points.end()); + return private_path; +} + +void MergeFromPrivateRoadModule::StateMachine::setState(State state) { state_ = state; } + +void MergeFromPrivateRoadModule::StateMachine::setMarginTime(const double t) { margin_time_ = t; } + +MergeFromPrivateRoadModule::State MergeFromPrivateRoadModule::StateMachine::getState() +{ + return state_; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp new file mode 100644 index 0000000000000..adda9e2189ae9 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -0,0 +1,542 @@ +// Copyright 2020 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +namespace util +{ +int insertPoint( + const geometry_msgs::msg::Pose & in_pose, + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path) +{ + static constexpr double dist_thr = 10.0; + static constexpr double angle_thr = M_PI / 1.5; + int closest_idx = -1; + if (!planning_utils::calcClosestIndex(*inout_path, in_pose, closest_idx, dist_thr, angle_thr)) { + return -1; + } + int insert_idx = closest_idx; + if (isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { + ++insert_idx; + } + + autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point; + inserted_point = inout_path->points.at(closest_idx); + inserted_point.point.pose = in_pose; + + auto it = inout_path->points.begin() + insert_idx; + inout_path->points.insert(it, inserted_point); + + return insert_idx; +} + +bool splineInterpolate( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger) +{ + *output = input; + + if (input.points.size() <= 1) { + RCLCPP_WARN(logger, "Do not interpolate because path size is 1."); + return false; + } + + static constexpr double ep = 1.0e-8; + + // calc arclength for path + std::vector base_x; + std::vector base_y; + std::vector base_z; + for (const auto & p : input.points) { + base_x.push_back(p.point.pose.position.x); + base_y.push_back(p.point.pose.position.y); + base_z.push_back(p.point.pose.position.z); + } + std::vector base_s = interpolation::calcEuclidDist(base_x, base_y); + + // remove duplicating sample points + { + size_t Ns = base_s.size(); + size_t i = 1; + while (i < Ns) { + if (std::fabs(base_s[i - 1] - base_s[i]) < ep) { + base_s.erase(base_s.begin() + i); + base_x.erase(base_x.begin() + i); + base_y.erase(base_y.begin() + i); + base_z.erase(base_z.begin() + i); + Ns -= 1; + i -= 1; + } + ++i; + } + } + + std::vector resampled_s; + for (double d = 0.0; d < base_s.back() - ep; d += interval) { + resampled_s.push_back(d); + } + + // do spline for xy + const std::vector resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s); + const std::vector resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s); + const std::vector resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s); + + // set xy + output->points.clear(); + for (size_t i = 0; i < resampled_s.size(); i++) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = resampled_x.at(i); + p.point.pose.position.y = resampled_y.at(i); + p.point.pose.position.z = resampled_z.at(i); + output->points.push_back(p); + } + + // set yaw + for (int i = 1; i < static_cast(resampled_s.size()) - 1; i++) { + auto p = output->points.at(i - 1).point.pose.position; + auto n = output->points.at(i + 1).point.pose.position; + double yaw = std::atan2(n.y - p.y, n.x - p.x); + output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); + } + if (output->points.size() > 1) { + size_t l = resampled_s.size(); + output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; + output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation; + } + return true; +} + +geometry_msgs::msg::Pose getAheadPose( + const size_t start_idx, const double ahead_dist, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.size() == 0) { + return geometry_msgs::msg::Pose{}; + } + + double curr_dist = 0.0; + double prev_dist = 0.0; + for (size_t i = start_idx; i < path.points.size() - 1; ++i) { + const geometry_msgs::msg::Pose p0 = path.points.at(i).point.pose; + const geometry_msgs::msg::Pose p1 = path.points.at(i + 1).point.pose; + curr_dist += planning_utils::calcDist2d(p0, p1); + if (curr_dist > ahead_dist) { + const double dl = std::max(curr_dist - prev_dist, 0.0001 /* avoid 0 divide */); + const double w_p0 = (curr_dist - ahead_dist) / dl; + const double w_p1 = (ahead_dist - prev_dist) / dl; + geometry_msgs::msg::Pose p; + p.position.x = w_p0 * p0.position.x + w_p1 * p1.position.x; + p.position.y = w_p0 * p0.position.y + w_p1 * p1.position.y; + p.position.z = w_p0 * p0.position.z + w_p1 * p1.position.z; + tf2::Quaternion q0_tf, q1_tf; + tf2::fromMsg(p0.orientation, q0_tf); + tf2::fromMsg(p1.orientation, q1_tf); + p.orientation = tf2::toMsg(q0_tf.slerp(q1_tf, w_p1)); + return p; + } + prev_dist = curr_dist; + } + return path.points.back().point.pose; +} + +bool setVelocityFrom( + const size_t idx, const double vel, autoware_auto_planning_msgs::msg::PathWithLaneId * input) +{ + for (size_t i = idx; i < input->points.size(); ++i) { + input->points.at(i).point.longitudinal_velocity_mps = + std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); + } + return true; +} + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); + bool is_target_ahead = (p.position.x > 0.0); + return is_target_ahead; +} + +bool hasLaneId(const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const int id) +{ + for (const auto & pid : p.lane_ids) { + if (pid == id) { + return true; + } + } + return false; +} + +bool hasDuplicatedPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & point, int * duplicated_point_idx) +{ + for (size_t i = 0; i < path.points.size(); i++) { + const auto & p = path.points.at(i).point.pose.position; + + constexpr double min_dist = 0.001; + if (planning_utils::calcDist2d(p, point) < min_dist) { + *duplicated_point_idx = static_cast(i); + return true; + } + } + + return false; +} + +int getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::vector & polygons) +{ + int first_idx_inside_lanelet = -1; + for (size_t i = 0; i < path.points.size(); ++i) { + bool is_in_lanelet = false; + auto p = path.points.at(i).point.pose.position; + for (const auto & polygon : polygons) { + const auto polygon_2d = lanelet::utils::to2D(polygon); + is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + first_idx_inside_lanelet = static_cast(i); + break; + } + } + if (is_in_lanelet) { + break; + } + } + return first_idx_inside_lanelet; +} + +bool generateStopLine( + const int lane_id, const std::vector detection_areas, + const std::shared_ptr & planner_data, + const IntersectionModule::PlannerParam & planner_param, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, int * stop_line_idx, + int * pass_judge_line_idx, int * first_idx_inside_lane, const rclcpp::Logger logger) +{ + /* set judge line dist */ + const double current_vel = planner_data->current_velocity->twist.linear.x; + const double max_acc = planner_data->max_stop_acceleration_threshold; + const double delay_response_time = planner_data->delay_response_time; + const double pass_judge_line_dist = + planning_utils::calcJudgeLineDistWithAccLimit(current_vel, max_acc, delay_response_time); + + /* set parameters */ + constexpr double interval = 0.2; + + const int margin_idx_dist = std::ceil(planner_param.stop_line_margin / interval); + const int base2front_idx_dist = + std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); + const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); + + /* spline interpolation */ + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!util::splineInterpolate(target_path, interval, &path_ip, logger)) { + return false; + } + + /* generate stop point */ + // If a stop_line is defined in lanelet_map, use it. + // else, generates a local stop_line with considering the lane conflicts. + int first_idx_ip_inside_lane; // first stop point index for interpolated path. + int stop_idx_ip; // stop point index for interpolated path. + if (getStopPoseIndexFromMap(path_ip, lane_id, planner_data, stop_idx_ip, 10.0, logger)) { + stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); + } else { + // get idx of first_inside_lane point + first_idx_ip_inside_lane = getFirstPointInsidePolygons(path_ip, detection_areas); + if (first_idx_ip_inside_lane == -1) { + RCLCPP_DEBUG(logger, "generate stopline, but no intersect line found."); + return false; + } + // only for visualization + const auto first_inside_point = path_ip.points.at(first_idx_ip_inside_lane).point.pose; + planning_utils::calcClosestIndex( + *original_path, first_inside_point, *first_idx_inside_lane, 10.0); + if (*first_idx_inside_lane == 0) { + RCLCPP_DEBUG( + logger, + "path[0] is already in the detection area. This happens if you have " + "already crossed the stop line or are very far from the intersection. Ignore computation."); + *stop_line_idx = 0; + *pass_judge_line_idx = 0; + return true; + } + stop_idx_ip = std::max(first_idx_ip_inside_lane - 1 - margin_idx_dist - base2front_idx_dist, 0); + } + + /* insert stop_point */ + const auto inserted_stop_point = path_ip.points.at(stop_idx_ip).point.pose; + // if path has too close (= duplicated) point to the stop point, do not insert it + // and consider the index of the duplicated point as stop_line_idx + if (!util::hasDuplicatedPoint(*original_path, inserted_stop_point.position, stop_line_idx)) { + *stop_line_idx = util::insertPoint(inserted_stop_point, original_path); + } + + /* if another stop point exist before intersection stop_line, disable judge_line. */ + bool has_prior_stopline = false; + for (int i = 0; i < *stop_line_idx; ++i) { + if (std::fabs(original_path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { + has_prior_stopline = true; + break; + } + } + + /* insert judge point */ + const int pass_judge_idx_ip = std::min( + static_cast(path_ip.points.size()) - 1, std::max(stop_idx_ip - pass_judge_idx_dist, 0)); + if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { + *pass_judge_line_idx = *stop_line_idx; + } else { + const auto inserted_pass_judge_point = path_ip.points.at(pass_judge_idx_ip).point.pose; + // if path has too close (= duplicated) point to the pass judge point, do not insert it + // and consider the index of the duplicated point as pass_judge_line_idx + if (!util::hasDuplicatedPoint( + *original_path, inserted_pass_judge_point.position, pass_judge_line_idx)) { + *pass_judge_line_idx = util::insertPoint(inserted_pass_judge_point, original_path); + ++(*stop_line_idx); // stop index is incremented by judge line insertion + } + } + + RCLCPP_DEBUG( + logger, + "generateStopLine() : stop_idx = %d, pass_judge_idx = %d, stop_idx_ip = " + "%d, pass_judge_idx_ip = %d, has_prior_stopline = %d", + *stop_line_idx, *pass_judge_line_idx, stop_idx_ip, pass_judge_idx_ip, has_prior_stopline); + + return true; +} + +bool getStopPoseIndexFromMap( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int lane_id, + const std::shared_ptr & planner_data, int & stop_idx_ip, int dist_thr, + const rclcpp::Logger logger) +{ + lanelet::ConstLanelet lanelet = planner_data->lanelet_map->laneletLayer.get(lane_id); + const auto road_markings = lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stop_line; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stop_line.push_back(road_marking->roadMarking()); + break; // only one stop_line exists. + } + } + if (stop_line.empty()) { + return false; + } + + const auto p_start = stop_line.front().front(); + const auto p_end = stop_line.front().back(); + const LineString2d extended_stop_line = + planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length); + + for (size_t i = 0; i < path.points.size() - 1; i++) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + std::vector collision_points; + bg::intersection(extended_stop_line, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + stop_idx_ip = i; + + RCLCPP_DEBUG(logger, "found collision point"); + + return true; + } + + geometry_msgs::msg::Point stop_point_from_map; + stop_point_from_map.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.z = 0.5 * (p_start.z() + p_end.z()); + + if (!planning_utils::calcClosestIndex(path, stop_point_from_map, stop_idx_ip, dist_thr)) { + RCLCPP_DEBUG(logger, "found stop line, but not found stop index"); + return false; + } + + RCLCPP_DEBUG(logger, "found stop line and stop index"); + + return true; +} + +bool getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const int lane_id, const IntersectionModule::PlannerParam & planner_param, + std::vector * conflicting_lanelets_result, + std::vector * objective_lanelets_result, const rclcpp::Logger logger) +{ + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); + + lanelet::ConstLanelets yield_lanelets; + + // for low priority lane + // If ego_lane has right of way (i.e. is high priority), + // ignore yieldLanelets (i.e. low priority lanes) + const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); + for (const auto & right_of_way : right_of_ways) { + if (lanelet::utils::contains(right_of_way->rightOfWayLanelets(), assigned_lanelet)) { + for (const auto & yield_lanelet : right_of_way->yieldLanelets()) { + yield_lanelets.push_back(yield_lanelet); + for (const auto & previous_lanelet : routing_graph_ptr->previous(yield_lanelet)) { + yield_lanelets.push_back(previous_lanelet); + } + } + } + } + + lanelet::ConstLanelets ego_lanelets; + + // for the behind ego-car lane. + for (const auto & previous_lanelet : routing_graph_ptr->previous(assigned_lanelet)) { + ego_lanelets.push_back(previous_lanelet); + for (const auto & following_lanelet : routing_graph_ptr->following(previous_lanelet)) { + if (lanelet::utils::contains(ego_lanelets, following_lanelet)) { + continue; + } + ego_lanelets.push_back(following_lanelet); + } + } + + // get conflicting lanes on assigned lanelet + const auto & conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + + std::vector // conflicting lanes with "lane_id" + conflicting_lanelets_ex_yield_ego; // excluding ego lanes and yield lanes + std::vector objective_lanelets; // final objective lanelets + + // exclude yield lanelets and ego lanelets from objective_lanelets + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + if (lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + conflicting_lanelets_ex_yield_ego.push_back({conflicting_lanelet}); + objective_lanelets.push_back({conflicting_lanelet}); + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + const double length = planner_param.detection_area_length; + std::vector objective_lanelets_sequences; + for (const auto & ll : objective_lanelets) { + // Preceding lanes does not include objective_lane so add them at the end + objective_lanelets_sequences.push_back(ll); + // get preceding lanelets without ego_lanelets + // to prevent the detection area from including the ego lanes and its' preceding lanes. + const auto lanelet_sequences = lanelet::utils::query::getPrecedingLaneletSequences( + routing_graph_ptr, ll.front(), length, ego_lanelets); + for (auto & l : lanelet_sequences) { + objective_lanelets_sequences.push_back(l); + } + } + + *conflicting_lanelets_result = conflicting_lanelets_ex_yield_ego; + *objective_lanelets_result = objective_lanelets_sequences; + + std::stringstream ss_c, ss_y, ss_e, ss_o, ss_os; + for (const auto & l : conflicting_lanelets) { + ss_c << l.id() << ", "; + } + for (const auto & l : yield_lanelets) { + ss_y << l.id() << ", "; + } + for (const auto & l : ego_lanelets) { + ss_e << l.id() << ", "; + } + for (const auto & l : objective_lanelets) { + ss_o << l.front().id() << ", "; + } + for (const auto & l : objective_lanelets_sequences) { + for (const auto & ll : l) { + ss_os << ll.id() << ", "; + } + } + RCLCPP_DEBUG( + logger, "getObjectiveLanelets() conflict = %s yield = %s ego = %s", ss_c.str().c_str(), + ss_y.str().c_str(), ss_e.str().c_str()); + RCLCPP_DEBUG( + logger, "getObjectiveLanelets() object = %s object_sequences = %s", ss_o.str().c_str(), + ss_os.str().c_str()); + return true; +} + +std::vector getPolygon3dFromLaneletsVec( + const std::vector & ll_vec, double clip_length) +{ + std::vector p_vec; + for (const auto & ll : ll_vec) { + const double path_length = lanelet::utils::getLaneletLength3d(ll); + const auto polygon3d = + lanelet::utils::getPolygonFromArcLength(ll, path_length - clip_length, path_length); + p_vec.push_back(polygon3d); + } + return p_vec; +} + +std::vector getLaneletIdsFromLaneletsVec(const std::vector & ll_vec) +{ + std::vector id_list; + for (const auto & lls : ll_vec) { + for (const auto & ll : lls) { + id_list.emplace_back(ll.id()); + } + } + return id_list; +} + +double calcArcLengthFromPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const size_t src_idx, + const size_t dst_idx) +{ + double length{0.0}; + for (size_t i = src_idx; i < dst_idx; ++i) { + const double dx_wp = input_path.points.at(i + 1).point.pose.position.x - + input_path.points.at(i).point.pose.position.x; + const double dy_wp = input_path.points.at(i + 1).point.pose.position.y - + input_path.points.at(i).point.pose.position.y; + length += std::hypot(dx_wp, dy_wp); + } + return length; +} + +} // namespace util +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp new file mode 100644 index 0000000000000..3855c67397337 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/debug.cpp @@ -0,0 +1,226 @@ +// Copyright 2021 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/no_stopping_area/scene_no_stopping_area.hpp" +#include "utilization/marker_helper.hpp" +#include "utilization/util.hpp" + +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +const double marker_lifetime = 0.2; +using DebugData = NoStoppingAreaModule::DebugData; + +lanelet::BasicPoint3d getCentroidPoint(const lanelet::BasicPolygon3d & poly) +{ + lanelet::BasicPoint3d p_sum{0.0, 0.0, 0.0}; + for (const auto & p : poly) { + p_sum += p; + } + return p_sum / poly.size(); +} + +geometry_msgs::msg::Point toMsg(const lanelet::BasicPoint3d & point) +{ + geometry_msgs::msg::Point msg; + msg.x = point.x(); + msg.y = point.y(); + msg.z = point.z(); + return msg; +} + +geometry_msgs::msg::Point toPoint2d(const geometry_msgs::msg::Point32 & poly) +{ + geometry_msgs::msg::Point msg; + msg.x = poly.x; + msg.y = poly.y; + msg.z = 0; + return msg; +} + +visualization_msgs::msg::MarkerArray createMarkerArray( + const DebugData & debug_data, const rclcpp::Time & now, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + const tf2::Transform tf_base_link2front( + tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0)); + + // Stop VirtualWall + const int32_t uid = planning_utils::bitShift(module_id); + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + geometry_msgs::msg::Pose pose; + tf2::toMsg(tf_map2front, pose); + msg = autoware_utils::createStopVirtualWallMarker(pose, "no_stopping_area", now, uid + j); + } + return msg; +} + +visualization_msgs::msg::MarkerArray createLaneletInfoMarkerArray( + const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + + // ID + { + auto marker = createDefaultMarker( + "map", now, "no_stopping_area_id", no_stopping_area_reg_elem.id(), + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerColor(1.0, 1.0, 1.0, 0.999)); + marker.scale = createMarkerScale(0.0, 0.0, 1.0); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + + for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { + const auto poly = detection_area.basicPolygon(); + + marker.pose.position = toMsg(poly.front()); + marker.pose.position.z += 2.0; + marker.text = std::to_string(no_stopping_area_reg_elem.id()); + + msg.markers.push_back(marker); + } + } + + // Polygon + { + auto marker = createDefaultMarker( + "map", now, "no_stopping_area_polygon", no_stopping_area_reg_elem.id(), + visualization_msgs::msg::Marker::LINE_LIST, createMarkerColor(0.1, 0.1, 1.0, 0.500)); + marker.scale = createMarkerScale(0.1, 0.0, 0.0); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + + for (const auto & no_stopping_area : no_stopping_area_reg_elem.noStoppingAreas()) { + const auto poly = no_stopping_area.basicPolygon(); + + for (size_t i = 0; i < poly.size(); ++i) { + const auto idx_front = i; + const auto idx_back = (i == poly.size() - 1) ? 0 : i + 1; + + const auto & p_front = poly.at(idx_front); + const auto & p_back = poly.at(idx_back); + + marker.points.push_back(toMsg(p_front)); + marker.points.push_back(toMsg(p_back)); + } + } + msg.markers.push_back(marker); + } + + const auto & stop_line = no_stopping_area_reg_elem.stopLine(); + // Polygon to StopLine + if (stop_line) { + const auto stop_line_center_point = + (stop_line.value().front().basicPoint() + stop_line.value().back().basicPoint()) / 2; + auto marker = createDefaultMarker( + "map", now, "no_stopping_area_correspondence", no_stopping_area_reg_elem.id(), + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerColor(0.1, 0.1, 1.0, 0.500)); + marker.scale = createMarkerScale(0.1, 0.0, 0.0); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + for (const auto & detection_area : no_stopping_area_reg_elem.noStoppingAreas()) { + const auto poly = detection_area.basicPolygon(); + const auto centroid_point = getCentroidPoint(poly); + for (size_t i = 0; i < poly.size(); ++i) { + marker.points.push_back(toMsg(centroid_point)); + marker.points.push_back(toMsg(stop_line_center_point)); + } + } + msg.markers.push_back(marker); + } + + return msg; +} + +visualization_msgs::msg::MarkerArray createStuckPointsMarkerArray( + const std::vector & stuck_points, const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + { + auto marker = createDefaultMarker( + "map", now, "stuck_points", 0, visualization_msgs::msg::Marker::SPHERE, + createMarkerColor(1.0, 1.0, 0.0, 0.999)); + marker.scale = createMarkerScale(0.3, 0.3, 0.3); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + for (size_t i = 0; i < stuck_points.size(); ++i) { + marker.id = i; + marker.pose.position = stuck_points.at(i); + msg.markers.push_back(marker); + } + } + return msg; +} + +visualization_msgs::msg::MarkerArray createNoStoppingAreaMarkerArray( + const geometry_msgs::msg::Polygon & stuck_vehicle_detect_area, const std::string & ns, + const rclcpp::Time & now) +{ + visualization_msgs::msg::MarkerArray msg; + { + auto marker = createDefaultMarker( + "map", now, ns.c_str(), 0, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerColor(1.0, 1.0, 0.0, 0.999)); + marker.scale = createMarkerScale(0.1, 0.1, 0.1); + marker.lifetime = rclcpp::Duration::from_seconds(marker_lifetime); + + for (size_t i = 0; i < stuck_vehicle_detect_area.points.size(); ++i) { + marker.id = i; + marker.points.emplace_back(toPoint2d(stuck_vehicle_detect_area.points[i])); + } + marker.points.emplace_back(toPoint2d(stuck_vehicle_detect_area.points.at(0))); + msg.markers.push_back(marker); + } + return msg; +} + +} // namespace + +visualization_msgs::msg::MarkerArray NoStoppingAreaModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + const rclcpp::Time current_time = clock_->now(); + appendMarkerArray( + createMarkerArray(debug_data_, current_time, getModuleId()), current_time, &debug_marker_array); + + appendMarkerArray( + createLaneletInfoMarkerArray(no_stopping_area_reg_elem_, current_time), current_time, + &debug_marker_array); + + if (!debug_data_.stuck_points.empty()) { + appendMarkerArray( + createStuckPointsMarkerArray(debug_data_.stuck_points, current_time), current_time, + &debug_marker_array); + } + if (!debug_data_.stuck_vehicle_detect_area.points.empty()) { + appendMarkerArray( + createNoStoppingAreaMarkerArray( + debug_data_.stuck_vehicle_detect_area, "stuck_vehicle_detect_area", current_time), + current_time, &debug_marker_array); + } + if (!debug_data_.stop_line_detect_area.points.empty()) { + appendMarkerArray( + createNoStoppingAreaMarkerArray( + debug_data_.stop_line_detect_area, "stop_line_detect_area", current_time), + current_time, &debug_marker_array); + } + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp new file mode 100644 index 0000000000000..df372e73e1406 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/manager.cpp @@ -0,0 +1,106 @@ +// Copyright 2021 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/no_stopping_area/manager.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::vector getNoStoppingAreaRegElemsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector no_stopping_area_reg_elems; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + const auto ll = lanelet_map->laneletLayer.get(lane_id); + const auto no_stopping_areas = + ll.regulatoryElementsAs(); + for (const auto & no_stopping_area : no_stopping_areas) { + no_stopping_area_reg_elems.push_back(no_stopping_area); + } + } + + return no_stopping_area_reg_elems; +} + +std::set getNoStoppingAreaIdSetOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::set no_stopping_area_id_set; + for (const auto & no_stopping_area : getNoStoppingAreaRegElemsOnPath(path, lanelet_map)) { + no_stopping_area_id_set.insert(no_stopping_area->id()); + } + return no_stopping_area_id_set; +} +} // namespace + +NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + auto & pp = planner_param_; + const auto & vi = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + pp.state_clear_time = node.declare_parameter(ns + ".state_clear_time", 2.0); + pp.stuck_vehicle_vel_thr = node.declare_parameter(ns + ".stuck_vehicle_vel_thr", 0.8333); + pp.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0); + pp.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 1.0); + pp.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + pp.detection_area_length = node.declare_parameter(ns + ".detection_area_length", 200.0); + pp.stuck_vehicle_front_margin = node.declare_parameter(ns + ".stuck_vehicle_front_margin", 6.0); + pp.path_expand_width = vi.vehicle_width_m * 0.5; +} + +void NoStoppingAreaModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & no_stopping_area : + getNoStoppingAreaRegElemsOnPath(path, planner_data_->lanelet_map)) { + // Use lanelet_id to unregister module when the route is changed + const auto module_id = no_stopping_area->id(); + if (!isModuleRegistered(module_id)) { + // assign 1 no stopping area for each module + registerModule(std::make_shared( + module_id, *no_stopping_area, planner_param_, logger_.get_child("no_stopping_area_module"), + clock_)); + } + } +} + +std::function &)> +NoStoppingAreaModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto no_stopping_area_id_set = + getNoStoppingAreaIdSetOnPath(path, planner_data_->lanelet_map); + + return [no_stopping_area_id_set](const std::shared_ptr & scene_module) { + return no_stopping_area_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp new file mode 100644 index 0000000000000..cf1c47a1e24db --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/no_stopping_area/scene_no_stopping_area.cpp @@ -0,0 +1,521 @@ +// Copyright 2021 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/no_stopping_area/scene_no_stopping_area.hpp" + +#include "utilization/arc_lane_util.hpp" +#include "utilization/interpolate.hpp" +#include "utilization/util.hpp" + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; +bool splineInterpolate( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger) +{ + *output = input; + + if (input.points.size() <= 1) { + RCLCPP_WARN(logger, "Do not interpolate because path size is 1."); + return false; + } + + static constexpr double ep = 1.0e-8; + + // calc arclength for path + std::vector base_x; + std::vector base_y; + std::vector base_z; + for (const auto & p : input.points) { + base_x.push_back(p.point.pose.position.x); + base_y.push_back(p.point.pose.position.y); + base_z.push_back(p.point.pose.position.z); + } + std::vector base_s = interpolation::calcEuclidDist(base_x, base_y); + + // remove duplicating sample points + { + size_t Ns = base_s.size(); + size_t i = 1; + while (i < Ns) { + if (std::fabs(base_s[i - 1] - base_s[i]) < ep) { + base_s.erase(base_s.begin() + i); + base_x.erase(base_x.begin() + i); + base_y.erase(base_y.begin() + i); + base_z.erase(base_z.begin() + i); + Ns -= 1; + i -= 1; + } + ++i; + } + } + + std::vector resampled_s; + for (double d = 0.0; d < base_s.back() - ep; d += interval) { + resampled_s.push_back(d); + } + + // do spline for xy + const std::vector resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s); + const std::vector resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s); + const std::vector resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s); + + // set xy + output->points.clear(); + for (size_t i = 0; i < resampled_s.size(); i++) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = resampled_x.at(i); + p.point.pose.position.y = resampled_y.at(i); + p.point.pose.position.z = resampled_z.at(i); + output->points.push_back(p); + } + + // set yaw + for (int i = 1; i < static_cast(resampled_s.size()) - 1; i++) { + auto p = output->points.at(i - 1).point.pose.position; + auto n = output->points.at(i + 1).point.pose.position; + double yaw = std::atan2(n.y - p.y, n.x - p.x); + output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); + } + if (output->points.size() > 1) { + size_t l = resampled_s.size(); + output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; + output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation; + } + return true; +} + +NoStoppingAreaModule::NoStoppingAreaModule( + const int64_t module_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + no_stopping_area_reg_elem_(no_stopping_area_reg_elem), + planner_param_(planner_param) +{ + state_machine_.setState(StateMachine::State::GO); + state_machine_.setMarginTime(planner_param_.state_clear_time); +} + +boost::optional NoStoppingAreaModule::getStopLineGeometry2d( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const double stop_line_margin) const +{ + // get stop line from map + { + const auto & stop_line = no_stopping_area_reg_elem_.stopLine(); + if (stop_line) { + return planning_utils::extendLine( + stop_line.value()[0], stop_line.value()[1], planner_data_->stop_line_extend_length); + } + } + // auto gen stop line + { + LineString2d stop_line; + /** + * @brief auto gen no stopping area stop line from area polygon if stop line is not set + * --------------- + * ------col-------------|--> ego path + * | Area | + * --------------- + **/ + + for (const auto & no_stopping_area : no_stopping_area_reg_elem_.noStoppingAreas()) { + const auto & area_poly = lanelet::utils::to2D(no_stopping_area).basicPolygon(); + lanelet::BasicLineString2d path_line; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + bg::intersection(area_poly, line, collision_points); + if (collision_points.empty()) { + continue; + } + const double yaw = autoware_utils::calcAzimuthAngle(p0, p1); + if (!collision_points.empty()) { + geometry_msgs::msg::Point left_point; + const double w = planner_data_->vehicle_info_.vehicle_width_m; + const double l = stop_line_margin; + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw + M_PI_2), + collision_points.front().y() + w * std::sin(yaw + M_PI_2)); + stop_line.emplace_back( + -l * std::cos(yaw) + collision_points.front().x() + w * std::cos(yaw - M_PI_2), + collision_points.front().y() + w * std::sin(yaw - M_PI_2)); + return stop_line; + } + } + } + } + return {}; +} + +bool NoStoppingAreaModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + // Store original path + const auto original_path = *path; + const auto & predicted_obj_arr_ptr = planner_data_->predicted_objects; + const auto & current_pose = planner_data_->current_pose; + if (path->points.size() <= 2) { + return true; + } + // Reset data + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::NO_STOPPING_AREA); + // Get stop line geometry + const auto stop_line = getStopLineGeometry2d(original_path, planner_param_.stop_line_margin); + if (!stop_line) { + return true; + } + const auto stop_point = + createTargetPoint(original_path, stop_line.value(), planner_param_.stop_margin); + if (!stop_point) { + return true; + } + const auto & stop_pose = stop_point->second; + if (isOverDeadLine(original_path, current_pose.pose, stop_pose)) { + // ego can't stop in front of no stopping area -> GO or OR + state_machine_.setState(StateMachine::State::GO); + return true; + } + const auto & vi = planner_data_->vehicle_info_; + const double margin = planner_param_.stop_line_margin; + const double ego_space_in_front_of_stuck_vehicle = + margin + vi.vehicle_length_m + planner_param_.stuck_vehicle_front_margin; + const Polygon2d stuck_vehicle_detect_area = generateEgoNoStoppingAreaLanePolygon( + *path, current_pose.pose, ego_space_in_front_of_stuck_vehicle, + planner_param_.detection_area_length); + const double ego_space_in_front_of_stop_line = + margin + planner_param_.stop_margin + vi.rear_overhang_m; + const Polygon2d stop_line_detect_area = generateEgoNoStoppingAreaLanePolygon( + *path, current_pose.pose, ego_space_in_front_of_stop_line, + planner_param_.detection_area_length); + if (stuck_vehicle_detect_area.outer().empty() && stop_line_detect_area.outer().empty()) { + return true; + } + debug_data_.stuck_vehicle_detect_area = toGeomMsg(stuck_vehicle_detect_area); + debug_data_.stop_line_detect_area = toGeomMsg(stop_line_detect_area); + // Find stuck vehicle in no stopping area + const bool is_entry_prohibited_by_stuck_vehicle = + checkStuckVehiclesInNoStoppingArea(stuck_vehicle_detect_area, predicted_obj_arr_ptr); + // Find stop line in no stopping area + const bool is_entry_prohibited_by_stop_line = + checkStopLinesInNoStoppingArea(*path, stop_line_detect_area); + const bool is_entry_prohibited = + is_entry_prohibited_by_stuck_vehicle || is_entry_prohibited_by_stop_line; + if (!isStoppable(current_pose.pose, stop_point->second)) { + state_machine_.setState(StateMachine::State::GO); + return false; + } else { + state_machine_.setStateWithMarginTime( + is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); + } + + if (state_machine_.getState() == StateMachine::State::STOP) { + // ----------------stop reason and stop point-------------------------- + insertStopPoint(*path, *stop_point); + // For virtual wall + debug_data_.stop_poses.push_back(stop_pose); + + // Create StopReason + { + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_point->second; + stop_factor.stop_factor_points = debug_data_.stuck_points; + planning_utils::appendStopReason(stop_factor, stop_reason); + } + + // Create legacy StopReason + { + const auto insert_idx = stop_point->first + 1; + if ( + !first_stop_path_point_index_ || + static_cast(insert_idx) < first_stop_path_point_index_) { + debug_data_.first_stop_pose = stop_pose; + first_stop_path_point_index_ = static_cast(insert_idx); + } + } + } else if (state_machine_.getState() == StateMachine::State::GO) { + // reset pass judge if current state is go + is_stoppable_ = true; + pass_judged_ = false; + } + return true; +} + +bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( + const Polygon2d & poly, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & + predicted_obj_arr_ptr) +{ + // stuck points by predicted objects + for (const auto & object : predicted_obj_arr_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v = std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x); + if (obj_v > planner_param_.stuck_vehicle_vel_thr) { + continue; // not stop vehicle + } + // check if the footprint is in the stuck detect area + const Polygon2d obj_footprint = planning_utils::toFootprintPolygon(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, poly); + if (is_in_stuck_area) { + RCLCPP_DEBUG(logger_, "stuck vehicle found."); + for (const auto p : obj_footprint.outer()) { + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = 0.0; + debug_data_.stuck_points.emplace_back(point); + } + return true; + } + } + return false; +} +bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) +{ + const double stop_vel = std::numeric_limits::min(); + // stuck points by stop line + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto p0 = path.points.at(i).point.pose.position; + const auto p1 = path.points.at(i + 1).point.pose.position; + const auto v0 = path.points.at(i).point.longitudinal_velocity_mps; + const auto v1 = path.points.at(i + 1).point.longitudinal_velocity_mps; + if (v0 > stop_vel && v1 > stop_vel) { + continue; + } + const LineString2d line{{p0.x, p0.y}, {p1.x, p1.y}}; + std::vector collision_points; + bg::intersection(poly, line, collision_points); + if (!collision_points.empty()) { + geometry_msgs::msg::Point point; + point.x = collision_points.front().x(); + point.y = collision_points.front().y(); + point.z = 0.0; + debug_data_.stuck_points.emplace_back(point); + return true; + } + } + return false; +} + +Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const +{ + Polygon2d ego_area; // open polygon + double dist_from_start_sum = 0.0; + const double interpolation_interval = 0.5; + bool is_in_area = false; + autoware_auto_planning_msgs::msg::PathWithLaneId interpolated_path; + if (!splineInterpolate(path, interpolation_interval, &interpolated_path, logger_)) { + return ego_area; + } + auto & pp = interpolated_path.points; + /* calc closest index */ + int closest_idx = -1; + if (!planning_utils::calcClosestIndex( + interpolated_path, ego_pose, closest_idx)) { + RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "calcClosestIndex fail"); + return ego_area; + } + const int num_ignore_nearest = 1; // Do not consider nearest lane polygon + size_t ego_area_start_idx = closest_idx + num_ignore_nearest; + size_t ego_area_end_idx = ego_area_start_idx; + // return if area size is not intentional + if (no_stopping_area_reg_elem_.noStoppingAreas().size() != 1) { + return ego_area; + } + const auto no_stopping_area = no_stopping_area_reg_elem_.noStoppingAreas().front(); + for (size_t i = closest_idx + num_ignore_nearest; i < pp.size() - 1; ++i) { + dist_from_start_sum += planning_utils::calcDist2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + is_in_area = true; + break; + } + if (dist_from_start_sum > extra_dist) { + return ego_area; + } + ++ego_area_start_idx; + } + if (ego_area_start_idx > num_ignore_nearest) { + ego_area_start_idx--; + } + if (!is_in_area) { + return ego_area; + } + double dist_from_area_sum = 0.0; + // decide end idx with extract distance + ego_area_end_idx = ego_area_start_idx; + for (size_t i = ego_area_start_idx; i < pp.size() - 1; ++i) { + dist_from_start_sum += planning_utils::calcDist2d(pp.at(i), pp.at(i - 1)); + const auto & p = pp.at(i).point.pose.position; + if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { + dist_from_area_sum += planning_utils::calcDist2d(pp.at(i), pp.at(i - 1)); + } + if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { + break; + } + ++ego_area_end_idx; + } + + const auto width = planner_param_.path_expand_width; + ego_area = planning_utils::generatePathPolygon( + interpolated_path, ego_area_start_idx, ego_area_end_idx, width); + return ego_area; +} + +bool NoStoppingAreaModule::isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const +{ + if ( + object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + +bool NoStoppingAreaModule::isOverDeadLine( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const +{ + return autoware_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) + + planner_param_.dead_line_margin < + 0.0; +} + +bool NoStoppingAreaModule::isStoppable( + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const +{ + // get vehicle info and compute pass_judge_line_distance + const auto current_velocity = planner_data_->current_velocity->twist.linear.x; + const auto current_acceleration = planner_data_->current_accel.get(); + const double max_acc = planner_data_->max_stop_acceleration_threshold; + const double max_jerk = planner_data_->max_stop_jerk_threshold; + const double delay_response_time = planner_data_->delay_response_time; + if (!planner_data_->current_accel) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 1000, + "[no stopping area] empty current acc! check current vel has been received."); + return false; + } + const double stoppable_distance = planning_utils::calcJudgeLineDistWithJerkLimit( + current_velocity, current_acceleration, max_acc, max_jerk, delay_response_time); + const double signed_arc_length = + arc_lane_utils::calcSignedDistance(self_pose, line_pose.position); + const bool distance_stoppable = stoppable_distance < signed_arc_length; + const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; + // ego vehicle is high speed and can't stop before stop line -> GO + const bool not_stoppable = !distance_stoppable && !slow_velocity; + // stoppable or not is judged only once + RCLCPP_DEBUG( + logger_, "stoppable_dist: %lf signed_arc_length: %lf", stoppable_distance, signed_arc_length); + if (!distance_stoppable && !pass_judged_) { + pass_judged_ = true; + // can't stop using maximum brake consider jerk limit + if (not_stoppable) { + // pass through + is_stoppable_ = false; + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 1000, "[NoStoppingArea] can't stop in front of no stopping area"); + } else { + is_stoppable_ = true; + } + } + return is_stoppable_; +} + +void NoStoppingAreaModule::insertStopPoint( + autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) +{ + const auto insert_idx = stop_point.first + 1; + const auto stop_pose = stop_point.second; + + // To PathPointWithLaneId + autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + stop_point_with_lane_id = path.points.at(insert_idx); + stop_point_with_lane_id.point.pose = stop_pose; + stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + + // Insert stop point + path.points.insert(path.points.begin() + insert_idx, stop_point_with_lane_id); + + // Insert 0 velocity after stop point + for (size_t j = insert_idx; j < path.points.size(); ++j) { + path.points.at(j).point.longitudinal_velocity_mps = 0.0; + } +} + +boost::optional NoStoppingAreaModule::createTargetPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const double margin) const +{ + // Find collision segment + const auto collision_segment = arc_lane_utils::findCollisionSegment(path, stop_line); + if (!collision_segment) { + // No collision + return {}; + } + + // Calculate offset length from stop line + // Use '-' to make the positive direction is forward + const double offset_length = -(margin + planner_data_->vehicle_info_.max_longitudinal_offset_m); + + // Find offset segment + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, *collision_segment, offset_length); + if (!offset_segment) { + // No enough path length + return {}; + } + + const auto front_idx = offset_segment->first; + const auto target_pose = arc_lane_utils::calcTargetPose(path, *offset_segment); + + return std::make_pair(front_idx, target_pose); +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp new file mode 100644 index 0000000000000..34ae06d5daa2c --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/debug.cpp @@ -0,0 +1,231 @@ +// Copyright 2021 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 +#include +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +visualization_msgs::msg::Marker makeArrowMarker( + const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, int id) +{ + visualization_msgs::msg::Marker debug_marker; + debug_marker.header.frame_id = "map"; + debug_marker.ns = "occlusion spot arrow"; + debug_marker.id = id; + debug_marker.type = visualization_msgs::msg::Marker::ARROW; + debug_marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.scale = autoware_utils::createMarkerScale(0.05, 0.2, 0.5); + debug_marker.color = autoware_utils::createMarkerColor(0.1, 0.1, 0.1, 0.5); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + geometry_msgs::msg::Point obs_point, intersection_point{}; + obs_point = possible_collision.obstacle_info.position; + obs_point.z += 1; + intersection_point = possible_collision.intersection_pose.position, intersection_point.z += 1; + debug_marker.points = {obs_point, intersection_point}; + return debug_marker; +} + +std::vector makeSlowDownMarkers( + const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, + const std::string road_type, int id) +{ + // virtual wall + std::vector debug_markers; + visualization_msgs::msg::Marker wall_marker; + wall_marker.header.frame_id = "map"; + wall_marker.ns = "occlusion spot slow down"; + wall_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + wall_marker.type = visualization_msgs::msg::Marker::CUBE; + wall_marker.action = visualization_msgs::msg::Marker::ADD; + wall_marker.id = id; + // cylinder at collision point + wall_marker.pose = possible_collision.intersection_pose; + wall_marker.pose.position.z += 1.0; + wall_marker.scale = autoware_utils::createMarkerScale(0.1, 5.0, 2.0); + wall_marker.color = autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 0.5); + + wall_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + debug_markers.emplace_back(wall_marker); + + // slow down reason marker + visualization_msgs::msg::Marker slowdown_reason_marker; + slowdown_reason_marker.header.frame_id = "map"; + slowdown_reason_marker.ns = "slow factor_text"; + slowdown_reason_marker.id = id; + slowdown_reason_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + slowdown_reason_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + slowdown_reason_marker.action = visualization_msgs::msg::Marker::ADD; + slowdown_reason_marker.pose = possible_collision.intersection_pose; + slowdown_reason_marker.scale = autoware_utils::createMarkerScale(0.0, 0.0, 1.0); + slowdown_reason_marker.color = autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.999); + slowdown_reason_marker.text = "occlusion spot"; + debug_markers.emplace_back(slowdown_reason_marker); + slowdown_reason_marker.scale = autoware_utils::createMarkerScale(0.0, 0.0, 0.5); + slowdown_reason_marker.id = id + 100; + slowdown_reason_marker.text = "\n \n" + road_type; + debug_markers.emplace_back(slowdown_reason_marker); + debug_markers.push_back(makeArrowMarker(possible_collision, id)); + return debug_markers; +} + +std::vector makeCollisionMarkers( + const occlusion_spot_utils::PossibleCollisionInfo & possible_collision, int id, bool show_text) +{ + std::vector debug_markers; + visualization_msgs::msg::Marker debug_marker; + debug_marker.header.frame_id = "map"; + debug_marker.ns = "collision_point"; + debug_marker.id = id; + // cylinder at collision_point point + debug_marker.type = visualization_msgs::msg::Marker::CYLINDER; + debug_marker.pose = possible_collision.collision_path_point.pose; + debug_marker.scale = autoware_utils::createMarkerScale(1.0, 1.0, 0.5); + debug_marker.color = autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.5); + + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.5); + debug_markers.push_back(debug_marker); + // cylinder at obstacle point + debug_marker.ns = "obstacle"; + debug_marker.type = visualization_msgs::msg::Marker::CYLINDER; + debug_marker.pose.position = possible_collision.obstacle_info.position; + debug_marker.color = autoware_utils::createMarkerColor(0.5, 0.5, 0.5, 0.5); + debug_marker.scale = autoware_utils::createMarkerScale(1.0, 1.0, 1.0); + debug_markers.push_back(debug_marker); + if (show_text) { + // info text at obstacle point + debug_marker.ns = "info_obstacle"; + debug_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + debug_marker.pose = possible_collision.collision_path_point.pose; + debug_marker.scale.z = 1.0; + debug_marker.color = autoware_utils::createMarkerColor(1.0, 1.0, 0.0, 1.0); + std::ostringstream string_stream; + string_stream << "(s,d,v)=(" << possible_collision.arc_lane_dist_at_collision.length << " , " + << possible_collision.arc_lane_dist_at_collision.distance << " , " + << possible_collision.collision_path_point.longitudinal_velocity_mps << ")"; + debug_marker.text = string_stream.str(); + debug_markers.push_back(debug_marker); + } + return debug_markers; +} + +std::vector makePolygonMarker( + const lanelet::BasicPolygon2d & polygon, double z, int id) +{ + std::vector debug_markers; + visualization_msgs::msg::Marker debug_marker; + debug_marker.header.frame_id = "map"; + debug_marker.id = id; + debug_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + debug_marker.action = visualization_msgs::msg::Marker::ADD; + debug_marker.pose.position = autoware_utils::createMarkerPosition(0.0, 0.0, z); + debug_marker.pose.orientation = autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); + debug_marker.scale = autoware_utils::createMarkerScale(0.05, 0.05, 0.05); + debug_marker.color = autoware_utils::createMarkerColor(1.0, 0.0, 1.0, 0.3); + debug_marker.lifetime = rclcpp::Duration::from_seconds(0.1); + debug_marker.ns = "sidewalk"; + for (const auto & p : polygon) { + geometry_msgs::msg::Point point = autoware_utils::createMarkerPosition(p.x(), p.y(), 0.0); + debug_marker.points.push_back(point); + } + debug_markers.push_back(debug_marker); + return debug_markers; +} + +template +visualization_msgs::msg::MarkerArray createMarkers( + T & debug_data, [[maybe_unused]] const int64_t module_id_) +{ + // add slow down markers for occlusion spot + visualization_msgs::msg::MarkerArray occlusion_spot_slowdown_markers; + auto & possible_collisions = debug_data.possible_collisions; + // sort collision + std::sort( + possible_collisions.begin(), possible_collisions.end(), + []( + occlusion_spot_utils::PossibleCollisionInfo pc1, + occlusion_spot_utils::PossibleCollisionInfo pc2) { + return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; + }); + + // draw virtual wall markers + int id = 0; + const double min_dist_between_markers = 5.0; + double prev_length = 0; + for (const auto & possible_collision : possible_collisions) { + if ( + possible_collision.arc_lane_dist_at_collision.length - prev_length > + min_dist_between_markers) { + prev_length = possible_collision.arc_lane_dist_at_collision.length; + std::vector collision_markers = + makeSlowDownMarkers(possible_collision, debug_data.road_type, id++); + occlusion_spot_slowdown_markers.markers.insert( + occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(), + collision_markers.end()); + } + } + + // draw obstacle collision + id = 0; + for (const auto & possible_collision : possible_collisions) { + // debug marker + std::vector collision_markers = + makeCollisionMarkers(possible_collision, id++, true); + occlusion_spot_slowdown_markers.markers.insert( + occlusion_spot_slowdown_markers.markers.end(), collision_markers.begin(), + collision_markers.end()); + } + + // draw slice if having sidewalk polygon + if (!debug_data.sidewalks.empty()) { + id = 0; + for (const auto & sidewalk : debug_data.sidewalks) { + for (const visualization_msgs::msg::Marker & m : + makePolygonMarker(sidewalk, debug_data.z, id++)) { + occlusion_spot_slowdown_markers.markers.push_back(m); + } + } + } + debug_data.sidewalks.clear(); + return occlusion_spot_slowdown_markers; +} + +} // namespace + +visualization_msgs::msg::MarkerArray OcclusionSpotInPublicModule::createDebugMarkerArray() +{ + const auto current_time = this->clock_->now(); + + visualization_msgs::msg::MarkerArray debug_marker_array; + appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array); + return debug_marker_array; +} +visualization_msgs::msg::MarkerArray OcclusionSpotInPrivateModule::createDebugMarkerArray() +{ + const auto current_time = this->clock_->now(); + + visualization_msgs::msg::MarkerArray debug_marker_array; + appendMarkerArray(createMarkers(debug_data_, module_id_), current_time, &debug_marker_array); + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp new file mode 100644 index 0000000000000..af314be864051 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/geometry.cpp @@ -0,0 +1,208 @@ +// Copyright 2021 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 +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace geometry +{ +void buildSlices( + std::vector & slices, const lanelet::ConstLanelet & path_lanelet, const SliceRange & range, + const double slice_length, const double slice_width) +{ + lanelet::BasicLineString2d path_boundary = path_lanelet.centerline2d().basicLineString(); + if (path_boundary.size() < 2) { + return; + } + const int num_lateral_slice = + static_cast(std::abs(range.max_distance - range.min_distance) / slice_width); + // ignore the last point + const int num_longitudinal_slice = + static_cast(std::abs(range.max_length - range.min_length) / slice_length); + slices.reserve(num_lateral_slice * num_longitudinal_slice); + // Note: offsetNoThrow is necessary + // as the sharp turn at the end of the trajectory can "reverse" the linestring + /** + * @brief + * build slice from occupancy grid : the first slice to create is ssss + * + * | 0 | 1 | 2 | 3 | 4 | + * 0 | --|---|---|---|---|-- outer bound + * 1 | | ? | ? | | | + * 2 | | ? | ? | | | + * 3 | | ? | ? | | | ^ + * 4 | s | s | ? | | | | dist_ratio + * 5 |-s-|-s-|---|---|---|-- inner bound --> length ratio + * Ego---------------------> total_slice_length + */ + lanelet::BasicLineString2d inner_bounds = path_boundary; + lanelet::BasicLineString2d outer_bounds; + outer_bounds = lanelet::geometry::offsetNoThrow(path_boundary, range.max_distance); + // correct self crossing with best effort + boost::geometry::correct(outer_bounds); + rclcpp::Clock clock{RCL_ROS_TIME}; + try { + // if correct couldn't solve self crossing skip this area + lanelet::geometry::internal::checkForInversion( + path_boundary, outer_bounds, std::abs(range.max_distance)); + } catch (...) { + RCLCPP_DEBUG_STREAM_THROTTLE( + rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"), clock, 5000, + "self crossing with offset " << range.max_distance); + } + // offset last point is especially messy + inner_bounds.pop_back(); + outer_bounds.pop_back(); + // resize to the same size as slice + inner_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size())); + outer_bounds.resize(std::min(inner_bounds.size(), outer_bounds.size())); + if (inner_bounds.size() < 2 || outer_bounds.size() < 2) { + return; + } + const double ratio_dist_start = std::abs(range.min_distance / range.max_distance); + const double ratio_dist_increment = std::min(1.0, slice_width / std::abs(range.max_distance)); + for (int s = 0; s < num_longitudinal_slice; s++) { + const double length = range.min_length + s * slice_length; + const double next_length = range.min_length + (s + 1.0) * slice_length; + const double min_length = + std::min(lanelet::geometry::length(outer_bounds), lanelet::geometry::length(inner_bounds)); + if (next_length > min_length) { + break; + } + for (int d = 0; d < num_lateral_slice; d++) { + const double ratio_dist = ratio_dist_start + d * ratio_dist_increment; + const double next_ratio_dist = ratio_dist_start + (d + 1.0) * ratio_dist_increment; + Slice slice; + buildInterpolatedPolygon( + slice.polygon, inner_bounds, outer_bounds, length, next_length, ratio_dist, + next_ratio_dist); + slice.range.min_length = length; + slice.range.max_length = next_length; + slice.range.min_distance = ratio_dist * range.max_distance; + slice.range.max_distance = next_ratio_dist * range.max_distance; + slices.emplace_back(slice); + } + } +} + +void buildInterpolatedPolygon( + lanelet::BasicPolygon2d & polygons, const lanelet::BasicLineString2d & inner_bounds, + const lanelet::BasicLineString2d & outer_bounds, const double current_length, + const double next_length, const double from_ratio_dist, const double to_ratio_dist) +{ + if (current_length >= next_length) { + RCLCPP_WARN( + rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot"), + "buildInterpolatedPolygon: starting length must be lower than target length"); + } + lanelet::BasicLineString2d inner_polygons; + lanelet::BasicLineString2d outer_polygons; + inner_polygons.reserve(inner_bounds.size()); + outer_polygons.reserve(outer_bounds.size()); + // Find starting point. Interpolate it if necessary + double length = 0.0; + size_t point_index = 0; + lanelet::BasicPoint2d inner_polygon_from; + lanelet::BasicPoint2d inner_polygon_to; + lanelet::BasicPoint2d outer_polygon_from; + lanelet::BasicPoint2d outer_polygon_to; + // Search first points of polygon + for (; length < current_length && point_index < inner_bounds.size() - 1; ++point_index) { + length += + lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]); + } + // if find current bound point index + if (length > current_length) { + const double length_between_points = + lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]); + const double length_ratio = + (length_between_points - (length - current_length)) / length_between_points; + inner_polygon_from = + lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio); + outer_polygon_from = + lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio); + } else { + inner_polygon_from = inner_bounds[point_index]; + outer_polygon_from = outer_bounds[point_index]; + if (length < next_length && point_index < inner_bounds.size() - 1) { + length += + lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]); + ++point_index; + } + } + inner_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, from_ratio_dist)); + outer_polygons.emplace_back(lerp(inner_polygon_from, outer_polygon_from, to_ratio_dist)); + + // Intermediate points + for (; length < next_length && point_index < inner_bounds.size() - 1; ++point_index) { + inner_polygons.emplace_back( + lerp(inner_bounds[point_index], outer_bounds[point_index], from_ratio_dist)); + outer_polygons.emplace_back( + lerp(inner_bounds[point_index], outer_bounds[point_index], to_ratio_dist)); + length += + lanelet::geometry::distance2d(inner_bounds[point_index], inner_bounds[point_index + 1]); + } + // Last points + if (length > next_length) { + const double length_between_points = + lanelet::geometry::distance2d(inner_bounds[point_index - 1], inner_bounds[point_index]); + const double length_ratio = + (length_between_points - (length - next_length)) / length_between_points; + inner_polygon_to = lerp(inner_bounds[point_index - 1], inner_bounds[point_index], length_ratio); + outer_polygon_to = lerp(outer_bounds[point_index - 1], outer_bounds[point_index], length_ratio); + } else { + inner_polygon_to = inner_bounds[point_index]; + outer_polygon_to = outer_bounds[point_index]; + } + inner_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, from_ratio_dist)); + outer_polygons.emplace_back(lerp(inner_polygon_to, outer_polygon_to, to_ratio_dist)); + // Build polygon + inner_polygons.insert(inner_polygons.end(), outer_polygons.rbegin(), outer_polygons.rend()); + polygons = lanelet::BasicPolygon2d(inner_polygons); +} + +std::vector buildSidewalkSlices( + const lanelet::ConstLanelet & path_lanelet, const double longitudinal_offset, + const double lateral_offset, const double slice_size, const double lateral_max_dist) +{ + std::vector slices; + std::vector left_slices; + std::vector right_slices; + const double longitudinal_max_dist = lanelet::geometry::length2d(path_lanelet); + geometry::SliceRange left_slice_range = { + longitudinal_offset, longitudinal_max_dist, lateral_offset, lateral_offset + lateral_max_dist}; + geometry::buildSlices(left_slices, path_lanelet, left_slice_range, slice_size, slice_size); + geometry::SliceRange right_slice_range = { + longitudinal_offset, longitudinal_max_dist, -lateral_offset, + -lateral_offset - lateral_max_dist}; + geometry::buildSlices(right_slices, path_lanelet, right_slice_range, slice_size, slice_size); + // Properly order lanelets from closest to furthest + for (size_t i = 0; i < right_slices.size(); ++i) { + slices.emplace_back(right_slices[i]); + } + for (size_t i = 0; i < left_slices.size(); ++i) { + slices.emplace_back(left_slices[i]); + } + + return slices; +} +} // namespace geometry +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp new file mode 100644 index 0000000000000..498c873b30d5f --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/grid_utils.cpp @@ -0,0 +1,199 @@ +// Copyright 2021 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 +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace grid_utils +{ +bool isOcclusionSpotSquare( + OcclusionSpotSquare & occlusion_spot, const grid_map::Matrix & grid_data, + const grid_map::Index & cell, int side_size, const grid_map::Size & grid_size) +{ + // Calculate ranges to check + int min_x; + int max_x; + int min_y; + int max_y; + // No occlusion_spot with size 0 + if (side_size == 0) { + return false; + } + /** + * @brief + * (min_x,min_y)...(max_x,min_y) + * . . + * (min_x,max_y)...(max_x,max_y) + */ + const int offset = side_size - 1; + min_x = cell.x(); + max_x = cell.x() + offset; + min_y = cell.y() - offset; + max_y = cell.y(); + // Ensure we stay inside the grid + min_x = std::max(0, min_x); + max_x = std::min(grid_size.x() - 1, max_x); + min_y = std::max(0, min_y); + max_y = std::min(grid_size.y() - 1, max_y); + for (int x = min_x; x <= max_x; ++x) { + for (int y = min_y; y <= max_y; ++y) { + // if the value is not unknown value return false + if (grid_data(x, y) != grid_utils::occlusion_cost_value::UNKNOWN) { + return false; + } + } + } + occlusion_spot.side_size = side_size; + occlusion_spot.index = cell; + return true; +} + +void findOcclusionSpots( + std::vector & occlusion_spot_positions, const grid_map::GridMap & grid, + const lanelet::BasicPolygon2d & polygon, double min_size) +{ + const grid_map::Matrix & grid_data = grid["layer"]; + const int min_occlusion_spot_size = std::max(0.0, std::floor(min_size / grid.getResolution())); + grid_map::Polygon grid_polygon; + for (const auto & point : polygon) { + grid_polygon.addVertex({point.x(), point.y()}); + } + for (grid_map::PolygonIterator iterator(grid, grid_polygon); !iterator.isPastEnd(); ++iterator) { + OcclusionSpotSquare occlusion_spot_square; + if (isOcclusionSpotSquare( + occlusion_spot_square, grid_data, *iterator, min_occlusion_spot_size, grid.getSize())) { + if (!grid.getPosition(occlusion_spot_square.index, occlusion_spot_square.position)) { + continue; + } + std::vector corner_positions; + getCornerPositions(corner_positions, grid, occlusion_spot_square); + for (const grid_map::Position & corner : corner_positions) { + occlusion_spot_positions.emplace_back(corner); + } + } + } +} + +bool isCollisionFree( + const grid_map::GridMap & grid, const grid_map::Position & p1, const grid_map::Position & p2) +{ + const grid_map::Matrix & grid_data = grid["layer"]; + try { + for (grid_map::LineIterator iterator(grid, p1, p2); !iterator.isPastEnd(); ++iterator) { + const grid_map::Index & index = *iterator; + if (grid_data(index.x(), index.y()) == grid_utils::occlusion_cost_value::OCCUPIED) { + return false; + } + } + } catch (const std::invalid_argument & e) { + std::cerr << e.what() << std::endl; + } + return true; +} + +void getCornerPositions( + std::vector & corner_positions, const grid_map::GridMap & grid, + const OcclusionSpotSquare & occlusion_spot_square) +{ + // Special case with size = 1: only one cell + if (occlusion_spot_square.side_size == 1) { + corner_positions.emplace_back(occlusion_spot_square.position); + return; + } + std::vector corner_indexes; + const int offset = (occlusion_spot_square.side_size - 1) / 2; + /** + * @brief relation of each grid position + * bl br + * tl tr + */ + corner_indexes = {// bl + grid_map::Index( + std::max(0, occlusion_spot_square.index.x() - offset), + std::max(0, occlusion_spot_square.index.y() - offset)), + // br + grid_map::Index( + std::min(grid.getSize().x() - 1, occlusion_spot_square.index.x() + offset), + std::max(0, occlusion_spot_square.index.y() - offset)), + // tl + grid_map::Index( + std::max(0, occlusion_spot_square.index.x() - offset), + std::min(grid.getSize().y() - 1, occlusion_spot_square.index.y() + offset)), + // tr + grid_map::Index( + std::min(grid.getSize().x() - 1, occlusion_spot_square.index.x() + offset), + std::min(grid.getSize().y() - 1, occlusion_spot_square.index.y() + offset))}; + for (const grid_map::Index & corner_index : corner_indexes) { + grid_map::Position corner_position; + grid.getPosition(corner_index, corner_position); + corner_positions.emplace_back(corner_position); + } +} +void imageToOccupancyGrid(const cv::Mat & cv_image, nav_msgs::msg::OccupancyGrid * occupancy_grid) +{ + const int width = cv_image.cols; + const int height = cv_image.rows; + occupancy_grid->data.clear(); + occupancy_grid->data.resize(width * height); + for (int x = width - 1; x >= 0; x--) { + for (int y = height - 1; y >= 0; y--) { + const int idx = (height - 1 - y) + (width - 1 - x) * height; + const unsigned char intensity = cv_image.at(y, x); + occupancy_grid->data.at(idx) = intensity; + } + } +} +void toQuantizedImage( + const nav_msgs::msg::OccupancyGrid & occupancy_grid, cv::Mat * cv_image, const GridParam & param) +{ + const int width = cv_image->cols; + const int height = cv_image->rows; + for (int x = width - 1; x >= 0; x--) { + for (int y = height - 1; y >= 0; y--) { + const int idx = (height - 1 - y) + (width - 1 - x) * height; + int8_t intensity = occupancy_grid.data.at(idx); + if (0 <= intensity && intensity < param.free_space_max) { + intensity = grid_utils::occlusion_cost_value::FREE_SPACE; + } else if ( // NOLINT + intensity == occlusion_cost_value::NO_INFORMATION || intensity < param.occupied_min) { + intensity = grid_utils::occlusion_cost_value::UNKNOWN; + } else { + intensity = grid_utils::occlusion_cost_value::OCCUPIED; + } + cv_image->at(y, x) = intensity; + } + } +} +void denoiseOccupancyGridCV( + nav_msgs::msg::OccupancyGrid & occupancy_grid, grid_map::GridMap & grid_map, + const GridParam & param) +{ + cv::Mat cv_image( + occupancy_grid.info.width, occupancy_grid.info.height, CV_8UC1, + cv::Scalar(grid_utils::occlusion_cost_value::OCCUPIED)); + toQuantizedImage(occupancy_grid, &cv_image, param); + constexpr int num_iter = 2; + //!< @brief opening & closing to remove noise in occupancy grid + cv::dilate(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), num_iter); + cv::erode(cv_image, cv_image, cv::Mat(), cv::Point(-1, -1), num_iter); + imageToOccupancyGrid(cv_image, &occupancy_grid); + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); +} +} // namespace grid_utils +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp new file mode 100644 index 0000000000000..28eb0f9ddd660 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp @@ -0,0 +1,153 @@ +// Copyright 2021 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 +#include +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::vector getLaneletsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector lanelets; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + lanelets.push_back(lanelet_map->laneletLayer.get(lane_id)); + } + return lanelets; +} + +bool hasPublicRoadOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { + // Is public load ? + const std::string location = ll.attributeOr("location", "else"); + if (location == "urban" || location == "public") { + return true; + } + } + return false; +} + +bool hasPrivateRoadOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + for (const auto & ll : getLaneletsOnPath(path, lanelet_map)) { + // Is private load ? + const std::string location = ll.attributeOr("location", "else"); + if (location == "private") { + return true; + } + } + return false; +} + +} // namespace + +OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + pub_debug_occupancy_grid_ = + node.create_publisher("~/debug/" + ns + "/occupancy_grid", 1); + + // for crosswalk parameters + auto & pp = planner_param_; + // assume pedestrian coming out from occlusion spot with this velocity + pp.pedestrian_vel = node.declare_parameter(ns + ".pedestrian_vel", 1.0); + pp.safety_time_buffer = node.declare_parameter(ns + ".safety_time_buffer", 0.1); + pp.detection_area_length = node.declare_parameter(ns + ".threshold.detection_area_length", 200.0); + pp.stuck_vehicle_vel = node.declare_parameter(ns + ".threshold.stuck_vehicle_vel", 1.0); + pp.lateral_distance_thr = node.declare_parameter(ns + ".threshold.lateral_distance", 10.0); + + pp.dist_thr = node.declare_parameter(ns + ".threshold.search_dist", 10.0); + pp.angle_thr = node.declare_parameter(ns + ".threshold.search_angle", M_PI / 5.0); + pp.show_debug_grid = node.declare_parameter(ns + ".show_debug_grid", false); + + // public road ego param + pp.public_road.min_velocity = node.declare_parameter(ns + ".public_road.min_velocity", 2.7); + pp.public_road.ebs_decel = node.declare_parameter(ns + ".public_road.ebs_decel", -5.0); + pp.public_road.pbs_decel = node.declare_parameter(ns + ".public_road.pbs_decel", -1.5); + + // private road + pp.private_road.min_velocity = node.declare_parameter(ns + ".private_road.min_velocity", 1.5); + pp.private_road.ebs_decel = node.declare_parameter(ns + ".private_road.ebs_decel", -4.0); + pp.private_road.pbs_decel = node.declare_parameter(ns + ".private_road.pbs_decel", -2.5); + + // sidewalk param + pp.sidewalk.min_occlusion_spot_size = + node.declare_parameter(ns + ".sidewalk.min_occlusion_spot_size", 2.0); + pp.sidewalk.focus_range = node.declare_parameter(ns + ".sidewalk.focus_range", 4.0); + pp.sidewalk.slice_size = node.declare_parameter(ns + ".sidewalk.slice_size", 1.5); + // occupancy grid param + pp.grid.free_space_max = node.declare_parameter(ns + ".grid.free_space_max", 10); + pp.grid.occupied_min = node.declare_parameter(ns + ".grid.occupied_min", 51); +} + +void OcclusionSpotModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const int64_t private_road_module_id = static_cast(ModuleID::PRIVATE); + const int64_t public_road_module_id = static_cast(ModuleID::PUBLIC); + // private + if (!isModuleRegistered(private_road_module_id)) { + if (hasPrivateRoadOnPath(path, planner_data_->lanelet_map)) { + registerModule(std::make_shared( + private_road_module_id, planner_data_, planner_param_, + logger_.get_child("occlusion_spot_in_private_module"), clock_, pub_debug_occupancy_grid_)); + } + } + // public + if (!isModuleRegistered(public_road_module_id)) { + if (hasPublicRoadOnPath(path, planner_data_->lanelet_map)) { + registerModule(std::make_shared( + public_road_module_id, planner_data_, planner_param_, + logger_.get_child("occlusion_spot_in_public_module"), clock_)); + } + } +} + +std::function &)> +OcclusionSpotModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const bool has_public_road = hasPublicRoadOnPath(path, planner_data_->lanelet_map); + const bool has_private_road = hasPrivateRoadOnPath(path, planner_data_->lanelet_map); + return [has_private_road, + has_public_road](const std::shared_ptr & scene_module) { + if (scene_module->getModuleId() == static_cast(ModuleID::PRIVATE)) { + return !has_private_road; + } + if (scene_module->getModuleId() == static_cast(ModuleID::PUBLIC)) { + return !has_public_road; + } + return true; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp new file mode 100644 index 0000000000000..1ac33fccb90a6 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp @@ -0,0 +1,511 @@ +// Copyright 2021 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 +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace occlusion_spot_utils +{ +bool splineInterpolate( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, + autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger) +{ + *output = input; + + if (input.points.size() <= 1) { + RCLCPP_WARN(logger, "Do not interpolate because path size is 1."); + return false; + } + + static constexpr double ep = 1.0e-8; + + // calc arclength for path + std::vector base_x; + std::vector base_y; + std::vector base_z; + for (const auto & p : input.points) { + base_x.push_back(p.point.pose.position.x); + base_y.push_back(p.point.pose.position.y); + base_z.push_back(p.point.pose.position.z); + } + std::vector base_s = interpolation::calcEuclidDist(base_x, base_y); + + // remove duplicating sample points + { + size_t Ns = base_s.size(); + size_t i = 1; + while (i < Ns) { + if (std::fabs(base_s[i - 1] - base_s[i]) < ep) { + base_s.erase(base_s.begin() + i); + base_x.erase(base_x.begin() + i); + base_y.erase(base_y.begin() + i); + base_z.erase(base_z.begin() + i); + Ns -= 1; + i -= 1; + } + ++i; + } + } + + std::vector resampled_s; + for (double d = 0.0; d < base_s.back() - ep; d += interval) { + resampled_s.push_back(d); + } + + // do spline for xy + const std::vector resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s); + const std::vector resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s); + const std::vector resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s); + + // set xy + output->points.clear(); + for (size_t i = 0; i < resampled_s.size(); i++) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + p.point.pose.position.x = resampled_x.at(i); + p.point.pose.position.y = resampled_y.at(i); + p.point.pose.position.z = resampled_z.at(i); + output->points.push_back(p); + } + + // set yaw + for (int i = 1; i < static_cast(resampled_s.size()) - 1; i++) { + auto p = output->points.at(i - 1).point.pose.position; + auto n = output->points.at(i + 1).point.pose.position; + double yaw = std::atan2(n.y - p.y, n.x - p.x); + output->points.at(i).point.pose.orientation = planning_utils::getQuaternionFromYaw(yaw); + } + if (output->points.size() > 1) { + size_t l = resampled_s.size(); + output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation; + output->points.back().point.pose.orientation = output->points.at(l - 1).point.pose.orientation; + } + return true; +} + +ROAD_TYPE getCurrentRoadType( + const lanelet::ConstLanelet & current_lanelet, + [[maybe_unused]] const lanelet::LaneletMapPtr & lanelet_map_ptr) +{ + const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot")}; + rclcpp::Clock clock{RCL_ROS_TIME}; + occlusion_spot_utils::ROAD_TYPE road_type; + std::string location; + if ( + current_lanelet.hasAttribute(lanelet::AttributeNamesString::Subtype) && + current_lanelet.attribute(lanelet::AttributeNamesString::Subtype) == + lanelet::AttributeValueString::Highway) { + location = "highway"; + } else { + location = current_lanelet.attributeOr("location", "else"); + } + RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "location: " << location); + if (location == "urban" || location == "public") { + road_type = occlusion_spot_utils::ROAD_TYPE::PUBLIC; + RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "public road: " << location); + } else if (location == "private") { + road_type = occlusion_spot_utils::ROAD_TYPE::PRIVATE; + RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "private road"); + } else if (location == "highway") { + road_type = occlusion_spot_utils::ROAD_TYPE::HIGHWAY; + RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "highway road"); + } else { + road_type = occlusion_spot_utils::ROAD_TYPE::UNKNOWN; + RCLCPP_DEBUG_STREAM_THROTTLE(logger, clock, 3000, "unknown road"); + } + return road_type; +} + +geometry_msgs::msg::Point setPoint(double x, double y, double z) +{ + geometry_msgs::msg::Point p; + p.x = x; + p.y = y; + p.z = z; + return p; +} + +void calcSlowDownPointsForPossibleCollision( + const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const double offset_from_ego_to_target, std::vector & possible_collisions) +{ + if (possible_collisions.empty()) { + return; + } + std::sort( + possible_collisions.begin(), possible_collisions.end(), + [](PossibleCollisionInfo pc1, PossibleCollisionInfo pc2) { + return pc1.arc_lane_dist_at_collision.length < pc2.arc_lane_dist_at_collision.length; + }); + + // get interpolated value between (s0,v0) - (s1,value) - (s2,v2) + auto getInterpolatedValue = [](double s0, double v0, double s1, double s2, double v2) { + if (s2 - s0 < std::numeric_limits::min()) { + return v0; + } + return v0 + (v2 - v0) * (s1 - s0) / (s2 - s0); + }; + // insert path point orientation to possible collision + size_t collision_index = 0; + double dist_along_path_point = offset_from_ego_to_target; + double dist_along_next_path_point = dist_along_path_point; + for (size_t idx = closest_idx; idx < path.points.size() - 1; idx++) { + auto p_prev = path.points[idx].point; + auto p_next = path.points[idx + 1].point; + const double dist_to_col = + possible_collisions.at(collision_index).arc_lane_dist_at_collision.length; + dist_along_next_path_point += + autoware_utils::calcDistance2d(p_prev.pose.position, p_next.pose.position); + // process if nearest possible collision is between current and next path point + if (dist_along_path_point < dist_to_col) { + for (; collision_index < possible_collisions.size(); collision_index++) { + const double d0 = dist_along_path_point; // distance at arc coordinate + const double d1 = dist_along_next_path_point; + const auto p0 = p_prev.pose.position; + const auto p1 = p_next.pose.position; + auto & col = possible_collisions[collision_index]; + auto & v = col.collision_path_point.longitudinal_velocity_mps; + const double current_dist2col = col.arc_lane_dist_at_collision.length; + v = getInterpolatedValue( + d0, p_prev.longitudinal_velocity_mps, dist_to_col, d1, p_next.longitudinal_velocity_mps); + const double x = getInterpolatedValue(d0, p0.x, dist_to_col, d1, p1.x); + const double y = getInterpolatedValue(d0, p0.y, dist_to_col, d1, p1.y); + const double z = getInterpolatedValue(d0, p0.z, dist_to_col, d1, p1.z); + // height is used to visualize marker correctly + + possible_collisions[collision_index].collision_path_point.pose.position = setPoint(x, y, z); + possible_collisions[collision_index].intersection_pose.position.z = z; + possible_collisions[collision_index].obstacle_info.position.z = z; + // break searching if dist to collision is farther than next path point + if (dist_along_next_path_point < current_dist2col) { + break; + } + } + if (collision_index == possible_collisions.size()) { + break; + } + } + dist_along_path_point = dist_along_next_path_point; + } +} + +autoware_auto_planning_msgs::msg::Path toPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) +{ + autoware_auto_planning_msgs::msg::Path path; + for (const auto & p : path_with_id.points) { + path.points.push_back(p.point); + } + return path; +} + +lanelet::ConstLanelet buildPathLanelet( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + autoware_auto_planning_msgs::msg::Path converted_path = filterLitterPathPoint(toPath(path)); + if (converted_path.points.empty()) { + return lanelet::ConstLanelet(); + } + // interpolation is done previously + lanelet::BasicLineString2d path_line; + path_line.reserve(converted_path.points.size()); + // skip last point that will creates extra ordinary path + for (size_t i = 0; i < converted_path.points.size() - 1; ++i) { + const auto & pose_i = converted_path.points[i].pose; + path_line.emplace_back(pose_i.position.x, pose_i.position.y); + } + // set simplified line to lanelet + lanelet::BasicLineString2d simplified_line; + boost::geometry::simplify(path_line, simplified_line, 0.1); + lanelet::Points3d path_points; + path_points.reserve(simplified_line.size()); + for (const auto & p : simplified_line) { + path_points.emplace_back(lanelet::InvalId, p.x(), p.y(), 0.0); + } + lanelet::LineString3d centerline(lanelet::InvalId, path_points); + lanelet::Lanelet path_lanelet(lanelet::InvalId); + path_lanelet.setCenterline(centerline); + return lanelet::ConstLanelet(path_lanelet); +} + +void calculateCollisionPathPointFromOcclusionSpot( + PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point, + const double offset_from_ego_to_target, const lanelet::ConstLanelet & path_lanelet, + const PlannerParam & param) +{ + auto calcSignedArcDistance = [](const double lateral_distance, const double offset) { + if (lateral_distance < 0) { + return lateral_distance + offset; + } + return lateral_distance - offset; + }; + lanelet::ArcCoordinates arc_lane_occlusion_point = + lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obstacle_point); + lanelet::BasicPoint2d intersection_point = lanelet::geometry::fromArcCoordinates( + path_lanelet.centerline2d(), {arc_lane_occlusion_point.length, 0.0}); + lanelet::BasicPoint2d col_point = lanelet::geometry::fromArcCoordinates( + path_lanelet.centerline2d(), + {arc_lane_occlusion_point.length - param.vehicle_info.baselink_to_front, 0.0}); + geometry_msgs::msg::Point search_point; + search_point.x = col_point[0]; + search_point.y = col_point[1]; + search_point.z = 0; + double path_angle = lanelet::utils::getLaneletAngle(path_lanelet, search_point); + tf2::Quaternion quat; + quat.setRPY(0, 0, path_angle); + autoware_auto_planning_msgs::msg::PathPoint collision_path_point; + pc.collision_path_point.pose.position.x = col_point[0]; + pc.collision_path_point.pose.position.y = col_point[1]; + pc.collision_path_point.pose.orientation = tf2::toMsg(quat); + ObstacleInfo obstacle_info; + pc.obstacle_info.position.x = obstacle_point[0]; + pc.obstacle_info.position.y = obstacle_point[1]; + pc.obstacle_info.max_velocity = param.pedestrian_vel; + geometry_msgs::msg::Pose intersection_pose; + pc.intersection_pose.position.x = intersection_point[0]; + pc.intersection_pose.position.y = intersection_point[1]; + pc.intersection_pose.orientation = tf2::toMsg(quat); + double signed_lateral_distance = calcSignedArcDistance( + arc_lane_occlusion_point.distance, param.vehicle_info.vehicle_width * 0.5); + pc.arc_lane_dist_at_collision = { + arc_lane_occlusion_point.length + offset_from_ego_to_target - + param.vehicle_info.baselink_to_front, + std::abs(signed_lateral_distance)}; +} + +void createPossibleCollisionBehindParkedVehicle( + std::vector & possible_collisions, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PlannerParam & param, + const double offset_from_ego_to_target, + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & dyn_obj_arr) +{ + lanelet::ConstLanelet path_lanelet = buildPathLanelet(path); + if (path_lanelet.centerline2d().empty()) { + return; + } + std::vector arcs; + auto calcSignedArcDistance = [](const double lateral_distance, const double offset) { + if (lateral_distance < 0) { + return lateral_distance + offset; + } + return lateral_distance - offset; + }; + const double half_vehicle_width = 0.5 * param.vehicle_info.vehicle_width; + for (const auto & dyn : dyn_obj_arr->objects) { + // consider if dynamic object is a car or bus or truck + if ( + dyn.classification.at(0).label != + autoware_auto_perception_msgs::msg::ObjectClassification::CAR && + dyn.classification.at(0).label != + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && + dyn.classification.at(0).label != + autoware_auto_perception_msgs::msg::ObjectClassification::BUS) { + continue; + } + const auto & state = dyn.kinematics; + // ignore if vehicle is moving + if (state.initial_twist_with_covariance.twist.linear.x > param.stuck_vehicle_vel) { + continue; + } + const geometry_msgs::msg::Point & p = dyn.kinematics.initial_pose_with_covariance.pose.position; + const geometry_msgs::msg::Quaternion & q = + dyn.kinematics.initial_pose_with_covariance.pose.orientation; + lanelet::BasicPoint2d obj_point; + obj_point[0] = p.x; + obj_point[1] = p.y; + lanelet::ArcCoordinates arc_lane_point_at_occlusion = + lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obj_point); + // add a half size of car to arc length as behind car + arc_lane_point_at_occlusion.length += dyn.shape.dimensions.x * 0.5; + // signed lateral distance used to convert obstacle position + double signed_lateral_distance = + calcSignedArcDistance(arc_lane_point_at_occlusion.distance, half_vehicle_width); + arc_lane_point_at_occlusion.distance = + std::abs(arc_lane_point_at_occlusion.distance) - 0.5 * dyn.shape.dimensions.y; + // ignore if obstacle is not within attention area + if ( + offset_from_ego_to_target + arc_lane_point_at_occlusion.length < + param.vehicle_info.baselink_to_front || + arc_lane_point_at_occlusion.distance < half_vehicle_width || + param.lateral_distance_thr + half_vehicle_width < arc_lane_point_at_occlusion.distance) { + continue; + } + // subtract baselink_to_front from longitudinal distance to occlusion point + double longitudinal_dist_at_collision_point = + arc_lane_point_at_occlusion.length - param.vehicle_info.baselink_to_front; + // collision point at baselink + lanelet::BasicPoint2d collision_point = lanelet::geometry::fromArcCoordinates( + path_lanelet.centerline2d(), {longitudinal_dist_at_collision_point, 0.0}); + geometry_msgs::msg::Point search_point; + search_point.x = collision_point[0]; + search_point.y = collision_point[1]; + search_point.z = 0; + double path_angle = lanelet::utils::getLaneletAngle(path_lanelet, search_point); + // ignore if angle is more different than 10[degree] + double obj_angle = tf2::getYaw(q); + const double diff_angle = autoware_utils::normalizeRadian(path_angle - obj_angle); + if (std::abs(diff_angle) > param.angle_thr) { + continue; + } + lanelet::BasicPoint2d obstacle_point = lanelet::geometry::fromArcCoordinates( + path_lanelet.centerline2d(), {arc_lane_point_at_occlusion.length, signed_lateral_distance}); + PossibleCollisionInfo pc; + calculateCollisionPathPointFromOcclusionSpot( + pc, obstacle_point, offset_from_ego_to_target, path_lanelet, param); + possible_collisions.emplace_back(pc); + } +} + +bool extractTargetRoad( + const int closest_idx, const lanelet::LaneletMapPtr lanelet_map_ptr, const double max_range, + const autoware_auto_planning_msgs::msg::PathWithLaneId & src_path, + double & offset_from_closest_to_target, + autoware_auto_planning_msgs::msg::PathWithLaneId & tar_path, const ROAD_TYPE & target_road_type) +{ + bool found_target = false; + // search lanelet that includes target_road_type only + for (size_t i = closest_idx; i < src_path.points.size(); i++) { + occlusion_spot_utils::ROAD_TYPE search_road_type = occlusion_spot_utils::getCurrentRoadType( + lanelet_map_ptr->laneletLayer.get(src_path.points[i].lane_ids[0]), lanelet_map_ptr); + if (found_target && search_road_type != target_road_type) { + break; + } + // ignore path farther than max range + if (offset_from_closest_to_target > max_range) { + break; + } + if (search_road_type == target_road_type) { + tar_path.points.emplace_back(src_path.points[i]); + found_target = true; + } + if (!found_target && i < src_path.points.size() - 1) { + const auto & curr_p = src_path.points[i].point.pose.position; + const auto & next_p = src_path.points[i + 1].point.pose.position; + offset_from_closest_to_target += autoware_utils::calcDistance2d(curr_p, next_p); + } + } + return found_target; +} + +void generatePossibleCollisions( + std::vector & possible_collisions, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const grid_map::GridMap & grid, + const double offset_from_ego_to_closest, const double offset_from_closest_to_target, + const PlannerParam & param, std::vector & debug) +{ + // NOTE : buildPathLanelet first index should always be zero because path is already limited + lanelet::ConstLanelet path_lanelet = buildPathLanelet(path); + if (path_lanelet.centerline2d().empty()) { + return; + } + // generate sidewalk possible collision + generateSidewalkPossibleCollisions( + possible_collisions, grid, offset_from_ego_to_closest, offset_from_closest_to_target, + path_lanelet, param, debug); + possible_collisions.insert( + possible_collisions.end(), possible_collisions.begin(), possible_collisions.end()); +} + +void generateSidewalkPossibleCollisions( + std::vector & possible_collisions, const grid_map::GridMap & grid, + const double offset_from_ego_to_closest, const double offset_from_closest_to_target, + const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param, + std::vector & debug) +{ + const double offset_form_ego_to_target = + offset_from_ego_to_closest + offset_from_closest_to_target; + double min_range = 0; + // if ego is inside the target path_lanelet, set ignore_length + if (offset_form_ego_to_target > param.vehicle_info.baselink_to_front) { + min_range = 0; + } else { + min_range = param.vehicle_info.baselink_to_front - offset_form_ego_to_target; + } + std::vector sidewalk_slices = geometry::buildSidewalkSlices( + path_lanelet, min_range, param.vehicle_info.vehicle_width * 0.5, param.sidewalk.slice_size, + param.sidewalk.focus_range); + double length_lower_bound = std::numeric_limits::max(); + double distance_lower_bound = std::numeric_limits::max(); + std::sort( + sidewalk_slices.begin(), sidewalk_slices.end(), + [](const geometry::Slice & s1, const geometry::Slice & s2) { + return s1.range.min_length < s2.range.min_length; + }); + + for (const geometry::Slice & sidewalk_slice : sidewalk_slices) { + debug.push_back(sidewalk_slice.polygon); + if ((sidewalk_slice.range.min_length < length_lower_bound || + std::abs(sidewalk_slice.range.min_distance) < distance_lower_bound)) { + std::vector occlusion_spot_positions; + grid_utils::findOcclusionSpots( + occlusion_spot_positions, grid, sidewalk_slice.polygon, + param.sidewalk.min_occlusion_spot_size); + generateSidewalkPossibleCollisionFromOcclusionSpot( + possible_collisions, grid, occlusion_spot_positions, offset_form_ego_to_target, + path_lanelet, param); + if (!possible_collisions.empty()) { + length_lower_bound = sidewalk_slice.range.min_length; + distance_lower_bound = std::abs(sidewalk_slice.range.min_distance); + possible_collisions.insert( + possible_collisions.end(), possible_collisions.begin(), possible_collisions.end()); + debug.push_back(sidewalk_slice.polygon); + } + } + } +} + +void generateSidewalkPossibleCollisionFromOcclusionSpot( + std::vector & possible_collisions, const grid_map::GridMap & grid, + const std::vector & occlusion_spot_positions, + const double offset_form_ego_to_target, const lanelet::ConstLanelet & path_lanelet, + const PlannerParam & param) +{ + for (grid_map::Position occlusion_spot_position : occlusion_spot_positions) { + // arc intersection + lanelet::BasicPoint2d obstacle_point = {occlusion_spot_position[0], occlusion_spot_position[1]}; + lanelet::ArcCoordinates arc_lane_point_at_occlusion = + lanelet::geometry::toArcCoordinates(path_lanelet.centerline2d(), obstacle_point); + lanelet::BasicPoint2d intersection_point = lanelet::geometry::fromArcCoordinates( + path_lanelet.centerline2d(), {arc_lane_point_at_occlusion.length, 0.0}); + bool collision_free = + grid_utils::isCollisionFree(grid, occlusion_spot_position, intersection_point); + if (collision_free) { + PossibleCollisionInfo pc; + calculateCollisionPathPointFromOcclusionSpot( + pc, obstacle_point, offset_form_ego_to_target, path_lanelet, param); + if (pc.arc_lane_dist_at_collision.length < param.vehicle_info.baselink_to_front) { + continue; + } + possible_collisions.emplace_back(pc); + break; + } + } +} + +} // namespace occlusion_spot_utils +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp new file mode 100644 index 0000000000000..de948e9c90259 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp @@ -0,0 +1,120 @@ +// Copyright 2021 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 +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace occlusion_spot_utils +{ +void applySafeVelocityConsideringPossibleCollison( + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + std::vector & possible_collisions, const double current_vel, + const EgoVelocity & ego, const PlannerParam & param) +{ + const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("occlusion_spot")}; + rclcpp::Clock clock{RCL_ROS_TIME}; + // return nullptr or too few points + if (!inout_path || inout_path->points.size() < 2) { + return; + } + for (auto & possible_collision : possible_collisions) { + const double dist_to_collision = possible_collision.arc_lane_dist_at_collision.length; + const double original_vel = possible_collision.collision_path_point.longitudinal_velocity_mps; + const double d_obs = possible_collision.arc_lane_dist_at_collision.distance; + const double v_obs = possible_collision.obstacle_info.max_velocity; + // skip if obstacle velocity is below zero + if (v_obs < 0) { + RCLCPP_WARN_THROTTLE( + logger, clock, 3000, "velocity for virtual darting object is not set correctly"); + continue; + // skip if distance to object is below zero + } else if (d_obs < 0) { + RCLCPP_WARN_THROTTLE( + logger, clock, 3000, "distance for virtual darting object is not set correctly"); + continue; + } + // RPB : risk predictive braking system velocity consider ego emergency braking deceleration + const double risk_predictive_braking_velocity = + calculateSafeRPBVelocity(param.safety_time_buffer, d_obs, v_obs, ego.ebs_decel); + + // PBS : predictive braking system velocity consider ego predictive braking deceleration + const double predictive_braking_system_velocity = + calculatePredictiveBrakingVelocity(current_vel, dist_to_collision, ego.pbs_decel); + + // get RPB velocity consider PBS limiter and minimum allowed velocity according to the road type + const double pbs_limited_rpb_vel = getPBSLimitedRPBVelocity( + predictive_braking_system_velocity, risk_predictive_braking_velocity, ego.min_velocity, + original_vel); + possible_collision.collision_path_point.longitudinal_velocity_mps = pbs_limited_rpb_vel; + insertSafeVelocityToPath( + possible_collision.collision_path_point.pose, pbs_limited_rpb_vel, param, inout_path); + } +} + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); + bool is_target_ahead = (p.position.x > 0.0); + return is_target_ahead; +} + +bool setVelocityFrom( + const size_t idx, const double vel, autoware_auto_planning_msgs::msg::PathWithLaneId * input) +{ + for (size_t i = idx; i < input->points.size(); ++i) { + input->points.at(i).point.longitudinal_velocity_mps = + std::min(static_cast(vel), input->points.at(i).point.longitudinal_velocity_mps); + } + return true; +} + +int insertSafeVelocityToPath( + const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param, + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path) +{ + int closest_idx = -1; + if (!planning_utils::calcClosestIndex( + *inout_path, in_pose, closest_idx, param.dist_thr, param.angle_thr)) { + return -1; + } + autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point; + inserted_point = inout_path->points.at(closest_idx); + int insert_idx = closest_idx; + // insert velocity to path if distance is not too close else insert new collision point + // if original path has narrow points it's better to set higher distance threshold + if (planning_utils::calcDist2d(in_pose, inserted_point.point) > 0.3) { + if (isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { + ++insert_idx; + } + // return if index is after the last path point + if (insert_idx == static_cast(inout_path->points.size())) { + return -1; + } + auto it = inout_path->points.begin() + insert_idx; + inserted_point = inout_path->points.at(closest_idx); + inserted_point.point.pose = in_pose; + inout_path->points.insert(it, inserted_point); + } + setVelocityFrom(insert_idx, safe_vel, inout_path); + return 0; +} + +} // namespace occlusion_spot_utils +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp new file mode 100644 index 0000000000000..0f18642fa5531 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_private_road.cpp @@ -0,0 +1,112 @@ +// Copyright 2020 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. + +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +OcclusionSpotInPrivateModule::OcclusionSpotInPrivateModule( + const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock, + const rclcpp::Publisher::SharedPtr publisher) +: SceneModuleInterface(module_id, logger, clock), publisher_(publisher) +{ + param_ = planner_param; +} + +bool OcclusionSpotInPrivateModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_planning_msgs::msg::StopReason * stop_reason) +{ + if (path->points.size() < 2) { + return true; + } + param_.vehicle_info.baselink_to_front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + param_.vehicle_info.vehicle_width = planner_data_->vehicle_info_.vehicle_width_m; + + const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; + const double ego_velocity = planner_data_->current_velocity->twist.linear.x; + const auto & lanelet_map_ptr = planner_data_->lanelet_map; + const auto & traffic_rules_ptr = planner_data_->traffic_rules; + const auto & occ_grid_ptr = planner_data_->occupancy_grid; + if (!lanelet_map_ptr || !traffic_rules_ptr || !occ_grid_ptr) { + return true; + } + + int closest_idx = -1; + if (!planning_utils::calcClosestIndex( + *path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { + return true; + } + + const auto target_road_type = occlusion_spot_utils::ROAD_TYPE::PRIVATE; + autoware_auto_planning_msgs::msg::PathWithLaneId limited_path; + double offset_from_ego_to_closest = 0; + double offset_from_closest_to_target = 0; + const double max_range = occ_grid_ptr->info.width * occ_grid_ptr->info.resolution; + { + // extract lanelet that includes target_road_type only + if (!behavior_velocity_planner::occlusion_spot_utils::extractTargetRoad( + closest_idx, lanelet_map_ptr, max_range, *path, offset_from_closest_to_target, + limited_path, target_road_type)) { + return true; + } + // use path point as origin for stability + offset_from_ego_to_closest = + -planning_utils::transformRelCoordinate2D(ego_pose, path->points[closest_idx].point.pose) + .position.x; + } + autoware_auto_planning_msgs::msg::PathWithLaneId interp_path; + occlusion_spot_utils::splineInterpolate(limited_path, 1.0, &interp_path, logger_); + if (interp_path.points.size() < 4) { + return true; + } + nav_msgs::msg::OccupancyGrid occ_grid = *occ_grid_ptr; + grid_map::GridMap grid_map; + grid_utils::denoiseOccupancyGridCV(occ_grid, grid_map, param_.grid); + if (param_.show_debug_grid) { + publisher_->publish(occ_grid); + } + std::vector possible_collisions; + const double offset_from_ego_to_target = + offset_from_ego_to_closest + offset_from_closest_to_target; + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 3000, "offset_from_ego_to_target : " << offset_from_ego_to_target); + occlusion_spot_utils::generatePossibleCollisions( + possible_collisions, interp_path, grid_map, offset_from_ego_to_closest, + offset_from_closest_to_target, param_, debug_data_.sidewalks); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 3000, "num possible collision:" << possible_collisions.size()); + behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision( + closest_idx, *path, offset_from_ego_to_target, possible_collisions); + // apply safe velocity using ebs and pbs deceleration + applySafeVelocityConsideringPossibleCollison( + path, possible_collisions, ego_velocity, param_.private_road, param_); + debug_data_.z = path->points.front().point.pose.position.z; + debug_data_.possible_collisions = possible_collisions; + return true; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp new file mode 100644 index 0000000000000..1f91e3e5bf6b3 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.cpp @@ -0,0 +1,101 @@ +// Copyright 2021 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 +#include +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +OcclusionSpotInPublicModule::OcclusionSpotInPublicModule( + const int64_t module_id, [[maybe_unused]] std::shared_ptr planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock) +{ + param_ = planner_param; +} + +bool OcclusionSpotInPublicModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] autoware_planning_msgs::msg::StopReason * stop_reason) +{ + if (path->points.size() < 2) { + return true; + } + param_.vehicle_info.baselink_to_front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + param_.vehicle_info.vehicle_width = planner_data_->vehicle_info_.vehicle_width_m; + + const geometry_msgs::msg::Pose ego_pose = planner_data_->current_pose.pose; + const double ego_velocity = planner_data_->current_velocity->twist.linear.x; + const auto & lanelet_map_ptr = planner_data_->lanelet_map; + const auto & traffic_rules_ptr = planner_data_->traffic_rules; + const auto & dynamic_obj_arr_ptr = planner_data_->predicted_objects; + if (!lanelet_map_ptr || !traffic_rules_ptr || !dynamic_obj_arr_ptr) { + return true; + } + int closest_idx = -1; + if (!planning_utils::calcClosestIndex( + *path, ego_pose, closest_idx, param_.dist_thr, param_.angle_thr)) { + return true; + } + const auto target_road_type = occlusion_spot_utils::ROAD_TYPE::PUBLIC; + autoware_auto_planning_msgs::msg::PathWithLaneId limited_path; + double offset_from_ego_to_closest = 0; + double offset_from_closest_to_target = 0; + { + // extract lanelet that includes target_road_type only + if (!behavior_velocity_planner::occlusion_spot_utils::extractTargetRoad( + closest_idx, lanelet_map_ptr, param_.detection_area_length, *path, + offset_from_closest_to_target, limited_path, target_road_type)) { + return true; + } + // use path point as origin for stability + offset_from_ego_to_closest = + -planning_utils::transformRelCoordinate2D(ego_pose, path->points[closest_idx].point.pose) + .position.x; + } + autoware_auto_planning_msgs::msg::PathWithLaneId interp_path; + occlusion_spot_utils::splineInterpolate(limited_path, 1.0, &interp_path, logger_); + if (interp_path.points.size() < 4) { + return true; + } + std::vector + possible_collisions; + RCLCPP_DEBUG_STREAM_THROTTLE(logger_, *clock_, 3000, "closest_idx : " << closest_idx); + RCLCPP_DEBUG_STREAM_THROTTLE( + logger_, *clock_, 3000, "offset_from_ego_to_closest : " << offset_from_ego_to_closest); + const double offset_from_ego_to_target = + offset_from_ego_to_closest + offset_from_closest_to_target; + behavior_velocity_planner::occlusion_spot_utils::createPossibleCollisionBehindParkedVehicle( + possible_collisions, interp_path, param_, offset_from_ego_to_target, dynamic_obj_arr_ptr); + // set orientation to each possible collision + behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision( + closest_idx, *path, offset_from_ego_to_target, possible_collisions); + // apply safe velocity using ebs and pbs deceleration + applySafeVelocityConsideringPossibleCollison( + path, possible_collisions, ego_velocity, param_.public_road, param_); + + debug_data_.possible_collisions = possible_collisions; + return true; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp new file mode 100644 index 0000000000000..56fb110283591 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/debug.cpp @@ -0,0 +1,97 @@ +// Copyright 2020 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 +#include +#include + +#include + +namespace behavior_velocity_planner +{ +namespace +{ +using DebugData = StopLineModule::DebugData; + +visualization_msgs::msg::MarkerArray createMarkers( + const DebugData & debug_data, const int64_t module_id) +{ + visualization_msgs::msg::MarkerArray msg; + tf2::Transform tf_base_link2front( + tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0)); + + // Stop VirtualWall + if (debug_data.stop_pose) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "stop_virtual_wall"; + marker.id = module_id; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(*debug_data.stop_pose, tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + + // Factor Text + if (debug_data.stop_pose) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "factor_text"; + marker.id = module_id; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(*debug_data.stop_pose, tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "stop line"; + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace + +visualization_msgs::msg::MarkerArray StopLineModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + appendMarkerArray( + createMarkers(debug_data_, module_id_), this->clock_->now(), &debug_marker_array); + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp new file mode 100644 index 0000000000000..ebb038c191f85 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/manager.cpp @@ -0,0 +1,116 @@ +// Copyright 2020 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 + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::vector getTrafficSignRegElemsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector traffic_sign_reg_elems; + + std::set unique_lane_ids; + for (const auto & p : path.points) { + unique_lane_ids.insert(p.lane_ids.at(0)); // should we iterate ids? keep as it was. + } + + for (const auto lane_id : unique_lane_ids) { + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + const auto tss = ll.regulatoryElementsAs(); + for (const auto & ts : tss) { + traffic_sign_reg_elems.push_back(ts); + } + } + + return traffic_sign_reg_elems; +} + +std::vector getStopLinesOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::vector stop_lines; + + for (const auto & traffic_sign_reg_elem : getTrafficSignRegElemsOnPath(path, lanelet_map)) { + // Is stop sign? + if (traffic_sign_reg_elem->type() != "stop_sign") { + continue; + } + + for (const auto & stop_line : traffic_sign_reg_elem->refLines()) { + stop_lines.push_back(stop_line); + } + } + + return stop_lines; +} + +std::set getStopLineIdSetOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::set stop_line_id_set; + + for (const auto & stop_line : getStopLinesOnPath(path, lanelet_map)) { + stop_line_id_set.insert(stop_line.id()); + } + + return stop_line_id_set; +} + +} // namespace + +StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + auto & p = planner_param_; + p.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0); + p.stop_check_dist = node.declare_parameter(ns + ".stop_check_dist", 2.0); + p.stop_duration_sec = node.declare_parameter(ns + ".stop_duration_sec", 1.0); +} + +void StopLineModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & stop_line : getStopLinesOnPath(path, planner_data_->lanelet_map)) { + const auto module_id = stop_line.id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, stop_line, planner_param_, logger_.get_child("stop_line_module"), clock_)); + } + } +} + +std::function &)> +StopLineModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->lanelet_map); + + return [stop_line_id_set](const std::shared_ptr & scene_module) { + return stop_line_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp new file mode 100644 index 0000000000000..6e66323603d94 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp @@ -0,0 +1,328 @@ +// Copyright 2020 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 +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +namespace +{ +double calcYawFromPoints( + const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back) +{ + return std::atan2(p_back.y - p_front.y, p_back.x - p_front.x); +} + +geometry_msgs::msg::Pose calcInterpolatedPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const StopLineModule::SegmentIndexWithOffset & offset_segment) +{ + // Get segment points + const auto & p_front = path.points.at(offset_segment.index).point.pose.position; + const auto & p_back = path.points.at(offset_segment.index + 1).point.pose.position; + + // To Eigen point + const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); + const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); + + // Calculate interpolation ratio + const auto interpolate_ratio = offset_segment.offset / (p_eigen_back - p_eigen_front).norm(); + + // Add offset to front point + const auto interpolated_point_2d = + p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); + const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); + + // Calculate orientation so that X-axis would be along the trajectory + tf2::Quaternion quat; + quat.setRPY(0, 0, calcYawFromPoints(p_front, p_back)); + + // To Pose + geometry_msgs::msg::Pose interpolated_pose; + interpolated_pose.position.x = interpolated_point_2d.x(); + interpolated_pose.position.y = interpolated_point_2d.y(); + interpolated_pose.position.z = interpolated_z; + interpolated_pose.orientation = tf2::toMsg(quat); + + return interpolated_pose; +} + +boost::optional findBackwardOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, + const double offset_length) +{ + double sum_length = 0.0; + const auto start = static_cast(base_idx) - 1; + for (std::int32_t i = start; i >= 0; --i) { + const auto p_front = to_bg2d(path.points.at(i).point.pose.position); + const auto p_back = to_bg2d(path.points.at(i + 1).point.pose.position); + + sum_length += bg::distance(p_front, p_back); + + // If it's over offset point, return front index and remain offset length + if (sum_length >= offset_length) { + const auto k = static_cast(i); + return StopLineModule::SegmentIndexWithOffset{k, sum_length - offset_length}; + } + } + + // No enough path length + return {}; +} + +StopLineModule::SegmentIndexWithOffset findNearestSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & point) +{ + std::vector distances; + distances.reserve(path.points.size()); + + std::transform( + path.points.cbegin(), path.points.cend(), std::back_inserter(distances), + [&point](const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) { + return planning_utils::calcDist2d(p.point.pose.position, point); + }); + + const auto min_itr = std::min_element(distances.cbegin(), distances.cend()); + const auto min_idx = static_cast(std::distance(distances.cbegin(), min_itr)); + + // Process boundary conditions + if (min_idx == 0 || min_idx == path.points.size() - 1) { + const size_t segment_idx = min_idx == 0 ? min_idx : min_idx - 1; + + return StopLineModule::SegmentIndexWithOffset{ + segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)}; + } + + // Process normal conditions(having previous and next points) + const auto & p_prev = path.points.at(min_idx - 1); + const auto d_prev = planning_utils::calcDist2d(p_prev, point); + + const auto & p_next = path.points.at(min_idx + 1); + const auto d_next = planning_utils::calcDist2d(p_next, point); + + const size_t segment_idx = (d_prev <= d_next) ? min_idx - 1 : min_idx; + + return StopLineModule::SegmentIndexWithOffset{ + segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)}; +} + +double calcSignedArcLength( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t & idx_front, + const size_t & idx_back) +{ + // If flipped, reverse index and take negative + if (idx_front > idx_back) { + return -calcSignedArcLength(path, idx_back, idx_front); + } + + double sum_length = 0.0; + for (size_t i = idx_front; i < idx_back; ++i) { + const auto & p1 = path.points.at(i).point.pose.position; + const auto & p2 = path.points.at(i + 1).point.pose.position; + sum_length += planning_utils::calcDist2d(p1, p2); + } + + return sum_length; +} + +double calcSignedArcLength( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + const auto seg_1 = findNearestSegment(path, p1); + const auto seg_2 = findNearestSegment(path, p2); + + return -seg_1.offset + calcSignedArcLength(path, seg_1.index, seg_2.index) + seg_2.offset; +} + +} // namespace + +StopLineModule::StopLineModule( + const int64_t module_id, const lanelet::ConstLineString3d & stop_line, + const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + module_id_(module_id), + stop_line_(stop_line), + state_(State::APPROACH) +{ + planner_param_ = planner_param; +} + +boost::optional StopLineModule::findCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line) +{ + for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto & p_front = path.points.at(i).point.pose.position; + const auto & p_back = path.points.at(i + 1).point.pose.position; + + // Find intersection + const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + std::vector collision_points; + bg::intersection(stop_line, path_segment, collision_points); + + // Ignore if no collision found + if (collision_points.empty()) { + continue; + } + + // Select first collision + const auto & collision_point = collision_points.at(0); + + return StopLineModule::SegmentIndexWithPoint2d{i, collision_point}; + } + + return {}; +} + +boost::optional StopLineModule::findOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const StopLineModule::SegmentIndexWithPoint2d & collision) +{ + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto base_backward_length = planner_param_.stop_margin + base_link2front; + + const auto & p_back = to_bg2d(path.points.at(collision.index + 1).point.pose.position); + + return findBackwardOffsetSegment( + path, collision.index + 1, base_backward_length + bg::distance(p_back, collision.point)); +} + +boost::optional StopLineModule::calcStopPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const boost::optional & offset_segment) +{ + // If no stop point found due to out of range, use front point on path + if (!offset_segment) { + return StopLineModule::SegmentIndexWithPose{0, path.points.front().point.pose}; + } + + return StopLineModule::SegmentIndexWithPose{ + offset_segment->index, calcInterpolatedPose(path, *offset_segment)}; +} + +autoware_auto_planning_msgs::msg::PathWithLaneId StopLineModule::insertStopPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const StopLineModule::SegmentIndexWithPose & stop_pose_with_index, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + auto modified_path = path; + + // Insert stop pose to between segment start and end + const int insert_index = stop_pose_with_index.index + 1; + auto stop_point_with_lane_id = modified_path.points.at(insert_index); + stop_point_with_lane_id.point.pose = stop_pose_with_index.pose; + stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + + // Update first stop index + first_stop_path_point_index_ = insert_index; + debug_data_.stop_pose = stop_point_with_lane_id.point.pose; + + // Insert stop point + modified_path.points.insert(modified_path.points.begin() + insert_index, stop_point_with_lane_id); + + // Insert 0 velocity after stop point + for (size_t j = insert_index; j < modified_path.points.size(); ++j) { + modified_path.points.at(j).point.longitudinal_velocity_mps = 0.0; + } + + // Get stop point and stop factor + { + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_point_with_lane_id.point.pose; + stop_factor.stop_factor_points.push_back(getCenterOfStopLine(stop_line_)); + planning_utils::appendStopReason(stop_factor, stop_reason); + } + + return modified_path; +} + +bool StopLineModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + first_stop_path_point_index_ = static_cast(path->points.size()) - 1; + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::STOP_LINE); + + const LineString2d stop_line = planning_utils::extendLine( + stop_line_[0], stop_line_[1], planner_data_->stop_line_extend_length); + // Find collision + const auto collision = findCollision(*path, stop_line); + + // If no collision found, do nothing + if (!collision) { + return true; + } + + // Find offset segment + const auto offset_segment = findOffsetSegment(*path, *collision); + + // Calculate stop pose and insert index + const auto stop_pose_with_index = calcStopPose(*path, offset_segment); + + // Calc dist to stop pose + const double signed_arc_dist_to_stop_point = calcSignedArcLength( + *path, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position); + + if (state_ == State::APPROACH) { + // Insert stop pose + *path = insertStopPose(*path, *stop_pose_with_index, stop_reason); + + // Move to stopped state if stopped + if ( + std::abs(signed_arc_dist_to_stop_point) < planner_param_.stop_check_dist && + planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { + RCLCPP_INFO(logger_, "APPROACH -> STOPPED"); + state_ = State::STOPPED; + } + } else if (state_ == State::STOPPED) { + // Change state after vehicle departure + if (!planner_data_->isVehicleStopped()) { + RCLCPP_INFO(logger_, "STOPPED -> START"); + state_ = State::START; + } + } else if (state_ == State::START) { + // Initialize if vehicle is far from stop_line + constexpr bool use_initialization_after_start = false; + if (use_initialization_after_start) { + if (signed_arc_dist_to_stop_point > planner_param_.stop_check_dist) { + RCLCPP_INFO(logger_, "START -> APPROACH"); + state_ = State::APPROACH; + } + } + } + + return true; +} + +geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( + const lanelet::ConstLineString3d & stop_line) +{ + geometry_msgs::msg::Point center_point; + center_point.x = (stop_line[0].x() + stop_line[1].x()) / 2.0; + center_point.y = (stop_line[0].y() + stop_line[1].y()) / 2.0; + center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; + return center_point; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp new file mode 100644 index 0000000000000..3e6677702b096 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/debug.cpp @@ -0,0 +1,144 @@ +// Copyright 2020 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 +#include +#include + +#include + +namespace behavior_velocity_planner +{ +namespace +{ +using DebugData = TrafficLightModule::DebugData; + +visualization_msgs::msg::MarkerArray createMarkerArray( + const DebugData & debug_data, const int64_t module_id) +{ + int32_t uid = planning_utils::bitShift(module_id); + visualization_msgs::msg::MarkerArray msg; + tf2::Transform tf_base_link2front( + tf2::Quaternion(0.0, 0.0, 0.0, 1.0), tf2::Vector3(debug_data.base_link2front, 0.0, 0.0)); + + // Stop VirtualWall + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "stop_virtual_wall"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + // Factor Text + for (size_t j = 0; j < debug_data.stop_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "factor_text"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.stop_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "traffic light"; + msg.markers.push_back(marker); + } + // Dead VirtualWall + for (size_t j = 0; j < debug_data.dead_line_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "dead line virtual_wall"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::CUBE; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.dead_line_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 1.0; + marker.scale.x = 0.1; + marker.scale.y = 5.0; + marker.scale.z = 2.0; + marker.color.a = 0.5; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + msg.markers.push_back(marker); + } + // Facto Text + for (size_t j = 0; j < debug_data.dead_line_poses.size(); ++j) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = "dead line factor_text"; + marker.id = uid + j; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + tf2::Transform tf_map2base_link; + tf2::fromMsg(debug_data.dead_line_poses.at(j), tf_map2base_link); + tf2::Transform tf_map2front = tf_map2base_link * tf_base_link2front; + tf2::toMsg(tf_map2front, marker.pose); + marker.pose.position.z += 2.0; + marker.scale.x = 0.0; + marker.scale.y = 0.0; + marker.scale.z = 1.0; + marker.color.a = 0.999; // Don't forget to set the alpha! + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.text = "traffic light (dead line)"; + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace + +visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + appendMarkerArray( + createMarkerArray(debug_data_, module_id_), this->clock_->now(), &debug_marker_array); + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp new file mode 100644 index 0000000000000..1181ed8a99267 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -0,0 +1,156 @@ +// Copyright 2020 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 + +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +std::unordered_map +getTrafficLightRegElemsOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::unordered_map traffic_light_reg_elems; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + const auto tls = ll.regulatoryElementsAs(); + for (const auto & tl : tls) { + traffic_light_reg_elems.insert(std::make_pair(tl, ll)); + } + } + + return traffic_light_reg_elems; +} + +std::set getLaneletIdSetOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::set lanelet_id_set; + for (const auto & traffic_light_reg_elem : getTrafficLightRegElemsOnPath(path, lanelet_map)) { + lanelet_id_set.insert(traffic_light_reg_elem.second.id()); + } + return lanelet_id_set; +} + +} // namespace + +TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + planner_param_.stop_margin = node.declare_parameter(ns + ".stop_margin", 0.0); + planner_param_.tl_state_timeout = node.declare_parameter(ns + ".tl_state_timeout", 1.0); + planner_param_.external_tl_state_timeout = + node.declare_parameter(ns + ".external_tl_state_timeout", 1.0); + planner_param_.enable_pass_judge = node.declare_parameter(ns + ".enable_pass_judge", true); + planner_param_.yellow_lamp_period = node.declare_parameter(ns + ".yellow_lamp_period", 2.75); + pub_tl_state_ = node.create_publisher( + "~/output/traffic_signal", 1); +} + +void TrafficLightModuleManager::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path) +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + autoware_planning_msgs::msg::StopReasonArray stop_reason_array; + autoware_auto_perception_msgs::msg::LookingTrafficSignal tl_state; + + tl_state.header.stamp = path->header.stamp; + tl_state.is_module_running = false; + + stop_reason_array.header.frame_id = "map"; + stop_reason_array.header.stamp = path->header.stamp; + first_stop_path_point_index_ = static_cast(path->points.size() - 1); + first_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); + for (const auto & scene_module : scene_modules_) { + autoware_planning_msgs::msg::StopReason stop_reason; + std::shared_ptr traffic_light_scene_module( + std::dynamic_pointer_cast(scene_module)); + traffic_light_scene_module->setPlannerData(planner_data_); + traffic_light_scene_module->modifyPathVelocity(path, &stop_reason); + stop_reason_array.stop_reasons.emplace_back(stop_reason); + if (traffic_light_scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) { + first_stop_path_point_index_ = traffic_light_scene_module->getFirstStopPathPointIndex(); + } + if ( + traffic_light_scene_module->getFirstRefStopPathPointIndex() < + first_ref_stop_path_point_index_) { + first_ref_stop_path_point_index_ = + traffic_light_scene_module->getFirstRefStopPathPointIndex(); + if ( + traffic_light_scene_module->getTrafficLightModuleState() != + TrafficLightModule::State::GO_OUT) { + tl_state = traffic_light_scene_module->getTrafficSignal(); + } + } + for (const auto & marker : traffic_light_scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + } + if (!stop_reason_array.stop_reasons.empty()) { + pub_stop_reason_->publish(stop_reason_array); + } + pub_debug_->publish(debug_marker_array); + pub_tl_state_->publish(tl_state); +} + +void TrafficLightModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & traffic_light_reg_elem : + getTrafficLightRegElemsOnPath(path, planner_data_->lanelet_map)) { + const auto stop_line = traffic_light_reg_elem.first->stopLine(); + + if (!stop_line) { + RCLCPP_FATAL( + logger_, "No stop line at traffic_light_reg_elem_id = %ld, please fix the map!", + traffic_light_reg_elem.first->id()); + continue; + } + + // Use lanelet_id to unregister module when the route is changed + const auto module_id = traffic_light_reg_elem.second.id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, + logger_.get_child("traffic_light_module"), clock_)); + } + } +} + +std::function &)> +TrafficLightModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lanelet_id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map); + + return [lanelet_id_set](const std::shared_ptr & scene_module) { + return lanelet_id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp new file mode 100644 index 0000000000000..00d12910449d3 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/scene.cpp @@ -0,0 +1,650 @@ +// Copyright 2020 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 +#include + +#include // To be replaced by std::optional in C++17 + +#include +#include + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +namespace +{ +std::pair findWayPointAndDistance( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const Eigen::Vector2d & p) +{ + constexpr double max_lateral_dist = 3.0; + for (size_t i = 0; i < input_path.points.size() - 1; ++i) { + const double dx = p.x() - input_path.points.at(i).point.pose.position.x; + const double dy = p.y() - input_path.points.at(i).point.pose.position.y; + const double dx_wp = input_path.points.at(i + 1).point.pose.position.x - + input_path.points.at(i).point.pose.position.x; + const double dy_wp = input_path.points.at(i + 1).point.pose.position.y - + input_path.points.at(i).point.pose.position.y; + + const double theta = std::atan2(dy, dx) - std::atan2(dy_wp, dx_wp); + + const double dist = std::hypot(dx, dy); + const double dist_wp = std::hypot(dx_wp, dy_wp); + + // check lateral distance + if (std::fabs(dist * std::sin(theta)) > max_lateral_dist) { + continue; + } + + // if the point p is back of the way point, return negative distance + if (dist * std::cos(theta) < 0) { + return std::make_pair(static_cast(i), -1.0 * dist); + } + + if (dist * std::cos(theta) < dist_wp) { + return std::make_pair(static_cast(i), dist); + } + } + + // if the way point is not found, return negative distance from the way point at 0 + const double dx = p.x() - input_path.points.at(0).point.pose.position.x; + const double dy = p.y() - input_path.points.at(0).point.pose.position.y; + return std::make_pair(-1, -1.0 * std::hypot(dx, dy)); +} + +double calcArcLengthFromWayPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const int & src, + const int & dst) +{ + double length = 0; + const size_t src_idx = src >= 0 ? static_cast(src) : 0; + const size_t dst_idx = dst >= 0 ? static_cast(dst) : 0; + for (size_t i = src_idx; i < dst_idx; ++i) { + const double dx_wp = input_path.points.at(i + 1).point.pose.position.x - + input_path.points.at(i).point.pose.position.x; + const double dy_wp = input_path.points.at(i + 1).point.pose.position.y - + input_path.points.at(i).point.pose.position.y; + length += std::hypot(dx_wp, dy_wp); + } + return length; +} + +double calcSignedArcLength( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const geometry_msgs::msg::Pose & p1, const Eigen::Vector2d & p2) +{ + std::pair src = + findWayPointAndDistance(input_path, Eigen::Vector2d(p1.position.x, p1.position.y)); + std::pair dst = findWayPointAndDistance(input_path, p2); + if (dst.first == -1) { + double dx = p1.position.x - p2.x(); + double dy = p1.position.y - p2.y(); + return -1.0 * std::hypot(dx, dy); + } + + if (src.first < dst.first) { + return calcArcLengthFromWayPoint(input_path, src.first, dst.first) - src.second + dst.second; + } else if (src.first > dst.first) { + return -1.0 * + (calcArcLengthFromWayPoint(input_path, dst.first, src.first) - dst.second + src.second); + } else { + return dst.second - src.second; + } +} + +[[maybe_unused]] double calcSignedDistance( + const geometry_msgs::msg::Pose & p1, const Eigen::Vector2d & p2) +{ + Eigen::Affine3d map2p1; + tf2::fromMsg(p1, map2p1); + auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x(), p2.y(), p1.position.z); + return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); +} + +bool getBackwardPointFromBasePoint( + const Eigen::Vector2d & line_point1, const Eigen::Vector2d & line_point2, + const Eigen::Vector2d & base_point, const double backward_length, Eigen::Vector2d & output_point) +{ + Eigen::Vector2d line_vec = line_point2 - line_point1; + Eigen::Vector2d backward_vec = backward_length * line_vec.normalized(); + output_point = base_point + backward_vec; + return true; +} + +boost::optional findNearestCollisionPoint( + const LineString2d & line1, const LineString2d & line2, const Point2d & origin) +{ + std::vector collision_points; + bg::intersection(line1, line2, collision_points); + + if (collision_points.empty()) { + return boost::none; + } + + // check nearest collision point + Point2d nearest_collision_point; + double min_dist = 0.0; + + for (size_t i = 0; i < collision_points.size(); ++i) { + double dist = bg::distance(origin, collision_points.at(i)); + if (i == 0 || dist < min_dist) { + min_dist = dist; + nearest_collision_point = collision_points.at(i); + } + } + return nearest_collision_point; +} + +bool createTargetPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const double offset, size_t & target_point_idx, Eigen::Vector2d & target_point) +{ + if (input.points.size() < 2) { + return false; + } + for (size_t i = 0; i < input.points.size() - 1; ++i) { + Point2d path_line_begin = { + input.points.at(i).point.pose.position.x, input.points.at(i).point.pose.position.y}; + Point2d path_line_end = { + input.points.at(i + 1).point.pose.position.x, input.points.at(i + 1).point.pose.position.y}; + LineString2d path_line = {path_line_begin, path_line_end}; + + // check nearest collision point + const auto nearest_collision_point = + findNearestCollisionPoint(path_line, stop_line, path_line_begin); + if (!nearest_collision_point) { + continue; + } + + // search target point index + target_point_idx = 0; + double length_sum = 0; + + Eigen::Vector2d point1, point2; + if (0 <= offset) { + point1 << nearest_collision_point->x(), nearest_collision_point->y(); + point2 << path_line_begin.x(), path_line_begin.y(); + length_sum += (point2 - point1).norm(); + for (size_t j = i; 0 < j; --j) { + if (offset < length_sum) { + target_point_idx = j + 1; + break; + } + point1 << input.points.at(j).point.pose.position.x, + input.points.at(j).point.pose.position.y; + point2 << input.points.at(j - 1).point.pose.position.x, + input.points.at(j - 1).point.pose.position.y; + length_sum += (point2 - point1).norm(); + } + } else { + point1 << nearest_collision_point->x(), nearest_collision_point->y(); + point2 << path_line_end.x(), path_line_end.y(); + length_sum -= (point2 - point1).norm(); + for (size_t j = i + 1; j < input.points.size() - 1; ++j) { + if (length_sum < offset) { + target_point_idx = j; + break; + } + point1 << input.points.at(j).point.pose.position.x, + input.points.at(j).point.pose.position.y; + point2 << input.points.at(j + 1).point.pose.position.x, + input.points.at(j + 1).point.pose.position.y; + length_sum -= (point2 - point1).norm(); + } + } + // create target point + getBackwardPointFromBasePoint( + point2, point1, point2, std::fabs(length_sum - offset), target_point); + return true; + } + return false; +} + +bool calcStopPointAndInsertIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, + const double & stop_line_extend_length, Eigen::Vector2d & stop_line_point, + size_t & stop_line_point_idx) +{ + LineString2d stop_line; + + for (size_t i = 0; i < lanelet_stop_lines.size() - 1; ++i) { + stop_line = planning_utils::extendLine( + lanelet_stop_lines[i], lanelet_stop_lines[i + 1], stop_line_extend_length); + + // Calculate stop pose and insert index, + // if there is a collision point between path and stop line + if (createTargetPoint(input_path, stop_line, offset, stop_line_point_idx, stop_line_point)) { + return true; + } + } + return false; +} + +geometry_msgs::msg::Point getTrafficLightPosition( + const lanelet::ConstLineStringOrPolygon3d & traffic_light) +{ + if (!traffic_light.lineString()) { + throw std::invalid_argument{"Traffic light is not LineString"}; + } + geometry_msgs::msg::Point tl_center; + auto traffic_light_ls = traffic_light.lineString().value(); + for (const auto & tl_point : traffic_light_ls) { + tl_center.x += tl_point.x() / traffic_light_ls.size(); + tl_center.y += tl_point.y() / traffic_light_ls.size(); + tl_center.z += tl_point.z() / traffic_light_ls.size(); + } + return tl_center; +} +autoware_auto_perception_msgs::msg::LookingTrafficSignal initializeTrafficSignal( + const rclcpp::Time stamp) +{ + autoware_auto_perception_msgs::msg::LookingTrafficSignal state; + state.header.stamp = stamp; + state.is_module_running = true; + state.perception.has_state = false; + state.external.has_state = false; + state.result.has_state = false; + return state; +} +} // namespace + +TrafficLightModule::TrafficLightModule( + const int64_t module_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + traffic_light_reg_elem_(traffic_light_reg_elem), + lane_(lane), + state_(State::APPROACH), + input_(Input::PERCEPTION), + is_prev_state_stop_(false) +{ + planner_param_ = planner_param; +} + +bool TrafficLightModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + looking_tl_state_ = initializeTrafficSignal(path->header.stamp); + debug_data_ = DebugData(); + debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + first_stop_path_point_index_ = static_cast(path->points.size()) - 1; + first_ref_stop_path_point_index_ = static_cast(path->points.size()) - 1; + *stop_reason = + planning_utils::initializeStopReason(autoware_planning_msgs::msg::StopReason::TRAFFIC_LIGHT); + + const auto input_path = *path; + + const auto & self_pose = planner_data_->current_pose; + + // Get lanelet2 traffic lights and stop lines. + lanelet::ConstLineString3d lanelet_stop_lines = *(traffic_light_reg_elem_.stopLine()); + lanelet::ConstLineStringsOrPolygons3d traffic_lights = traffic_light_reg_elem_.trafficLights(); + + // Calculate stop pose and insert index + Eigen::Vector2d stop_line_point{}; + size_t stop_line_point_idx{}; + if (!calcStopPointAndInsertIndex( + input_path, lanelet_stop_lines, + planner_param_.stop_margin + planner_data_->vehicle_info_.max_longitudinal_offset_m, + planner_data_->stop_line_extend_length, stop_line_point, stop_line_point_idx)) { + RCLCPP_WARN_THROTTLE(logger_, *clock_, 5000, "Failed to calculate stop point and insert index"); + return false; + } + + // Calculate dist to stop pose + const double signed_arc_length_to_stop_point = + calcSignedArcLength(input_path, self_pose.pose, stop_line_point); + + // Check state + if (state_ == State::APPROACH) { + // Move to go out state if ego vehicle over deadline. + constexpr double signed_deadline_length = -2.0; + if (signed_arc_length_to_stop_point < signed_deadline_length) { + RCLCPP_INFO(logger_, "APPROACH -> GO_OUT"); + state_ = State::GO_OUT; + return true; + } + + first_ref_stop_path_point_index_ = stop_line_point_idx; + + // Check if stop is coming. + if (!isStopSignal(traffic_lights)) { + is_prev_state_stop_ = false; + return true; + } + + // Decide whether to stop or pass even if a stop signal is received. + if (!isPassthrough(signed_arc_length_to_stop_point)) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + is_prev_state_stop_ = true; + } + return true; + } else if (state_ == State::GO_OUT) { + // Initialize if vehicle is far from stop_line + constexpr bool use_initialization_after_start = true; + constexpr double restart_length = 1.0; + if (use_initialization_after_start) { + if (signed_arc_length_to_stop_point > restart_length) { + RCLCPP_INFO(logger_, "GO_OUT(RESTART) -> APPROACH"); + state_ = State::APPROACH; + } + } + return true; + } + + return false; +} + +bool TrafficLightModule::isStopSignal(const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +{ + if (!updateTrafficSignal(traffic_lights)) { + return false; + } + + return looking_tl_state_.result.judge == + autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP; +} + +bool TrafficLightModule::updateTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights) +{ + autoware_auto_perception_msgs::msg::TrafficSignalStamped tl_state_perception; + autoware_auto_perception_msgs::msg::TrafficSignalStamped tl_state_external; + bool found_perception = getHighestConfidenceTrafficSignal(traffic_lights, tl_state_perception); + bool found_external = getExternalTrafficSignal(traffic_lights, tl_state_external); + + if (!found_perception && !found_external) { + // Don't stop when UNKNOWN or TIMEOUT as discussed at #508 + input_ = Input::NONE; + return false; + } + + if (found_perception) { + looking_tl_state_.perception = generateTlStateWithJudgeFromTlState(tl_state_perception.signal); + looking_tl_state_.result = looking_tl_state_.perception; + input_ = Input::PERCEPTION; + } + + if (found_external) { + looking_tl_state_.external = generateTlStateWithJudgeFromTlState(tl_state_external.signal); + looking_tl_state_.result = looking_tl_state_.external; + input_ = Input::EXTERNAL; + } + + return true; +} + +bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const +{ + const double max_acc = planner_data_->max_stop_acceleration_threshold; + const double max_jerk = planner_data_->max_stop_jerk_threshold; + const double delay_response_time = planner_data_->delay_response_time; + + const double reachable_distance = + planner_data_->current_velocity->twist.linear.x * planner_param_.yellow_lamp_period; + + if (!planner_data_->current_accel) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 1000, + "[traffic_light] empty current acc! check current vel has been received."); + return false; + } + // Calculate distance until ego vehicle decide not to stop, + // taking into account the jerk and acceleration. + const double pass_judge_line_distance = planning_utils::calcJudgeLineDistWithJerkLimit( + planner_data_->current_velocity->twist.linear.x, planner_data_->current_accel.get(), max_acc, + max_jerk, delay_response_time); + + const bool distance_stoppable = pass_judge_line_distance < signed_arc_length; + const bool slow_velocity = planner_data_->current_velocity->twist.linear.x < 2.0; + const bool stoppable = distance_stoppable || slow_velocity; + const bool reachable = signed_arc_length < reachable_distance; + + const auto & enable_pass_judge = planner_param_.enable_pass_judge; + + if (enable_pass_judge && !stoppable && !is_prev_state_stop_ && input_ == Input::PERCEPTION) { + // Cannot stop under acceleration and jerk limits. + // However, ego vehicle can't enter the intersection while the light is yellow. + // It is called dilemma zone. Make a stop decision to be safe. + if (!reachable) { + // dilemma zone: emergency stop + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 1000, + "[traffic_light] cannot pass through intersection during yellow lamp!"); + return false; + } else { + // pass through + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 1000, "[traffic_light] can pass through intersection during yellow lamp"); + return true; + } + } else { + return false; + } +} + +bool TrafficLightModule::isTrafficSignalStop( + const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state) const +{ + if (hasTrafficLightColor(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::GREEN)) { + return false; + } + + const std::string turn_direction = lane_.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && + hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, autoware_auto_perception_msgs::msg::TrafficLight::UP_ARROW)) { + return false; + } + + return true; +} + +bool TrafficLightModule::getHighestConfidenceTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, + autoware_auto_perception_msgs::msg::TrafficSignalStamped & highest_confidence_tl_state) +{ + // search traffic light state + bool found = false; + double highest_confidence = 0.0; + std::string reason; + for (const auto & traffic_light : traffic_lights) { + // traffic light must be linestrings + if (!traffic_light.isLineString()) { + reason = "NotLineString"; + continue; + } + + const int id = static_cast(traffic_light).id(); + RCLCPP_DEBUG(logger_, "traffic light id: %d (on route)", id); + const auto tl_state_stamped = planner_data_->getTrafficSignal(id); + if (!tl_state_stamped) { + reason = "TrafficSignalNotFound"; + continue; + } + + const auto header = tl_state_stamped->header; + const auto tl_state = tl_state_stamped->signal; + if (!((clock_->now() - header.stamp).seconds() < planner_param_.tl_state_timeout)) { + reason = "TimeOut"; + continue; + } + + if ( + tl_state.lights.empty() || + tl_state.lights.front().color == autoware_auto_perception_msgs::msg::TrafficLight::UNKNOWN) { + reason = "TrafficLightUnknown"; + continue; + } + + if (highest_confidence < tl_state.lights.front().confidence) { + highest_confidence = tl_state.lights.front().confidence; + highest_confidence_tl_state = *tl_state_stamped; + try { + debug_data_.traffic_light_points.push_back(getTrafficLightPosition(traffic_light)); + debug_data_.highest_confidence_traffic_light_point = getTrafficLightPosition(traffic_light); + } catch (const std::invalid_argument & ex) { + RCLCPP_WARN_STREAM(logger_, ex.what()); + continue; + } + found = true; + } + } + if (!found) { + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 1000 /* ms */, "cannot find traffic light lamp state (%s).", + reason.c_str()); + return false; + } + return true; +} + +autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId modified_path; + modified_path = input; + + // Create stop pose + const int target_velocity_point_idx = std::max(static_cast(insert_target_point_idx - 1), 0); + auto target_point_with_lane_id = modified_path.points.at(target_velocity_point_idx); + target_point_with_lane_id.point.pose.position.x = target_point.x(); + target_point_with_lane_id.point.pose.position.y = target_point.y(); + target_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; + + // Insert stop pose into path + modified_path.points.insert( + modified_path.points.begin() + insert_target_point_idx, target_point_with_lane_id); + debug_data_.stop_poses.push_back(target_point_with_lane_id.point.pose); + + // insert 0 velocity after target point + for (size_t j = insert_target_point_idx; j < modified_path.points.size(); ++j) { + modified_path.points.at(j).point.longitudinal_velocity_mps = 0.0; + } + if (target_velocity_point_idx < first_stop_path_point_index_) { + first_stop_path_point_index_ = target_velocity_point_idx; + debug_data_.first_stop_pose = target_point_with_lane_id.point.pose; + } + + // Get stop point and stop factor + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = debug_data_.first_stop_pose; + stop_factor.stop_factor_points = + std::vector{debug_data_.highest_confidence_traffic_light_point}; + planning_utils::appendStopReason(stop_factor, stop_reason); + + return modified_path; +} + +bool TrafficLightModule::hasTrafficLightColor( + const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, + const uint8_t & lamp_color) const +{ + const auto it_lamp = std::find_if( + tl_state.lights.begin(), tl_state.lights.end(), + [&lamp_color](const auto & x) { return x.color == lamp_color; }); + + return it_lamp != tl_state.lights.end(); +} + +bool TrafficLightModule::hasTrafficLightShape( + const autoware_auto_perception_msgs::msg::TrafficSignal & tl_state, + const uint8_t & lamp_shape) const +{ + const auto it_lamp = std::find_if( + tl_state.lights.begin(), tl_state.lights.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.lights.end(); +} + +bool TrafficLightModule::getExternalTrafficSignal( + const lanelet::ConstLineStringsOrPolygons3d & traffic_lights, + autoware_auto_perception_msgs::msg::TrafficSignalStamped & external_tl_state) +{ + // search traffic light state + bool found = false; + std::string reason; + for (const auto & traffic_light : traffic_lights) { + // traffic light must be linestrings + if (!traffic_light.isLineString()) { + reason = "NotLineString"; + continue; + } + + const int id = static_cast(traffic_light).id(); + const auto tl_state_stamped = planner_data_->getExternalTrafficSignal(id); + if (!tl_state_stamped) { + reason = "TrafficSignalNotFound"; + continue; + } + + const auto header = tl_state_stamped->header; + const auto tl_state = tl_state_stamped->signal; + if (!((clock_->now() - header.stamp).seconds() < planner_param_.external_tl_state_timeout)) { + reason = "TimeOut"; + continue; + } + + external_tl_state = *tl_state_stamped; + found = true; + } + if (!found) { + RCLCPP_DEBUG_THROTTLE( + logger_, *clock_, 5000 /* ms */, + "[traffic_light] cannot find external traffic light lamp state (%s).", reason.c_str()); + return false; + } + return true; +} + +autoware_auto_perception_msgs::msg::TrafficSignalWithJudge +TrafficLightModule::generateTlStateWithJudgeFromTlState( + const autoware_auto_perception_msgs::msg::TrafficSignal tl_state) const +{ + autoware_auto_perception_msgs::msg::TrafficSignalWithJudge tl_state_with_judge; + tl_state_with_judge.signal = tl_state; + tl_state_with_judge.has_state = true; + tl_state_with_judge.judge = isTrafficSignalStop(tl_state) + ? autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::STOP + : autoware_auto_perception_msgs::msg::TrafficSignalWithJudge::GO; + return tl_state_with_judge; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp new file mode 100644 index 0000000000000..997a6fdea5569 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/debug.cpp @@ -0,0 +1,143 @@ +// Copyright 2021 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 +#include + +using autoware_utils::appendMarkerArray; +using autoware_utils::createDefaultMarker; +using autoware_utils::createMarkerColor; +using autoware_utils::createMarkerOrientation; +using autoware_utils::createMarkerPosition; +using autoware_utils::createMarkerScale; +using autoware_utils::createStopVirtualWallMarker; +using autoware_utils::toMsg; +using namespace std::literals::string_literals; + +namespace behavior_velocity_planner +{ +namespace +{ +[[maybe_unused]] autoware_utils::LinearRing3d createCircle( + const autoware_utils::Point3d & p, const double radius, const size_t num_points = 50) +{ + autoware_utils::LinearRing3d ring; // clockwise and closed + + for (size_t i = 0; i < num_points; ++i) { + const double theta = i * (2 * autoware_utils::pi / num_points); + const double x = p.x() + radius * std::sin(theta); + const double y = p.y() + radius * std::cos(theta); + ring.emplace_back(x, y, p.z()); + } + + // Make closed + ring.emplace_back(p.x(), p.y() + radius, p.z()); + + return ring; +} +} // namespace + +visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray debug_marker_array; + + // Common + const auto & m = map_data_; + const auto & d = module_data_; + const auto now = clock_->now(); + + // instrument_id + { + auto marker = createDefaultMarker( + "map", now, "instrument_id", module_id_, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + marker.pose.position = toMsg(m.instrument_center); + marker.text = m.instrument_id; + + debug_marker_array.markers.push_back(marker); + } + + // instrument_center + { + auto marker = createDefaultMarker( + "map", now, "instrument_center", module_id_, visualization_msgs::msg::Marker::SPHERE, + createMarkerScale(0.3, 0.3, 0.3), createMarkerColor(1.0, 0.0, 0.0, 0.999)); + + marker.pose.position = toMsg(m.instrument_center); + + debug_marker_array.markers.push_back(marker); + } + + // stop_line + if (m.stop_line) { + auto marker = createDefaultMarker( + "map", now, "stop_line", module_id_, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + for (const auto & p : *m.stop_line) { + marker.points.push_back(toMsg(p)); + } + + debug_marker_array.markers.push_back(marker); + } + + // start_line + { + auto marker = createDefaultMarker( + "map", now, "start_line", module_id_, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & p : m.start_line) { + marker.points.push_back(toMsg(p)); + } + + debug_marker_array.markers.push_back(marker); + } + + // end_lines + { + auto marker = createDefaultMarker( + "map", now, "end_lines", module_id_, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.3, 0.0, 0.0), createMarkerColor(0.0, 1.0, 1.0, 0.999)); + + for (const auto & line : m.end_lines) { + for (size_t i = 1; i < line.size(); ++i) { + marker.points.push_back(toMsg(line.at(i - 1))); + marker.points.push_back(toMsg(line.at(i))); + } + } + + debug_marker_array.markers.push_back(marker); + } + + // virtual_wall_stop_line + if (d.stop_head_pose_at_stop_line) { + const auto markers = createStopVirtualWallMarker( + *d.stop_head_pose_at_stop_line, "virtual_traffic_light", now, module_id_); + + appendMarkerArray(markers, &debug_marker_array); + } + + // virtual_wall_end_line + if (d.stop_head_pose_at_end_line) { + const auto markers = createStopVirtualWallMarker( + *d.stop_head_pose_at_end_line, "virtual_traffic_light", now, module_id_); + + appendMarkerArray(markers, &debug_marker_array); + } + + return debug_marker_array; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp new file mode 100644 index 0000000000000..0dc04d1d11b3f --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -0,0 +1,99 @@ +// Copyright 2021 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 + +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +using lanelet::autoware::VirtualTrafficLight; + +template +std::unordered_map getRegElemMapOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::unordered_map reg_elem_map_on_path; + + for (const auto & p : path.points) { + const auto lane_id = p.lane_ids.at(0); + const auto ll = lanelet_map->laneletLayer.get(lane_id); + + for (const auto & reg_elem : ll.regulatoryElementsAs()) { + reg_elem_map_on_path.insert(std::make_pair(reg_elem, ll)); + } + } + + return reg_elem_map_on_path; +} + +std::set getLaneletIdSetOnPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const lanelet::LaneletMapPtr lanelet_map) +{ + std::set id_set; + + for (const auto & m : getRegElemMapOnPath(path, lanelet_map)) { + id_set.insert(m.second.id()); + } + + return id_set; +} + +} // namespace + +VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(getModuleName()); + + { + auto & p = planner_param_; + p.max_delay_sec = node.declare_parameter(ns + ".max_delay_sec", 3.0); + } +} + +void VirtualTrafficLightModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + for (const auto & m : + getRegElemMapOnPath(path, planner_data_->lanelet_map)) { + // Use lanelet_id to unregister module when the route is changed + const auto module_id = m.second.id(); + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, *m.first, m.second, planner_param_, + logger_.get_child("virtual_traffic_light_module"), clock_)); + } + } +} + +std::function &)> +VirtualTrafficLightModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto id_set = getLaneletIdSetOnPath(path, planner_data_->lanelet_map); + + return [id_set](const std::shared_ptr & scene_module) { + return id_set.count(scene_module->getModuleId()) == 0; + }; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp new file mode 100644 index 0000000000000..f3676118bdf60 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/scene.cpp @@ -0,0 +1,601 @@ +// Copyright 2021 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 +#include +#include + +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace +{ +namespace bg = boost::geometry; +using autoware_utils::calcDistance2d; + +struct SegmentIndexWithPoint +{ + size_t index; + geometry_msgs::msg::Point point; +}; + +struct SegmentIndexWithOffset +{ + size_t index; + double offset; +}; + +autoware_v2x_msgs::msg::KeyValue createKeyValue(const std::string & key, const std::string & value) +{ + return autoware_v2x_msgs::build().key(key).value(value); +} + +autoware_utils::LineString3d toAutowarePoints(const lanelet::ConstLineString3d & line_string) +{ + autoware_utils::LineString3d output; + for (const auto & p : line_string) { + output.emplace_back(p.x(), p.y(), p.z()); + } + return output; +} + +boost::optional toAutowarePoints( + const lanelet::Optional & line_string) +{ + if (!line_string) { + return {}; + } + return toAutowarePoints(*line_string); +} + +std::vector toAutowarePoints( + const lanelet::ConstLineStrings3d & line_strings) +{ + std::vector output; + for (const auto & line_string : line_strings) { + output.emplace_back(toAutowarePoints(line_string)); + } + return output; +} + +autoware_utils::LineString2d to_2d(const autoware_utils::LineString3d & line_string) +{ + autoware_utils::LineString2d output; + for (const auto & p : line_string) { + output.emplace_back(p.x(), p.y()); + } + return output; +} + +autoware_utils::Point3d calcCenter(const autoware_utils::LineString3d & line_string) +{ + const auto p1 = line_string.front(); + const auto p2 = line_string.back(); + const auto p_center = (p1 + p2) / 2; + return {p_center.x(), p_center.y(), p_center.z()}; +} + +geometry_msgs::msg::Pose calcHeadPose( + const geometry_msgs::msg::Pose & base_link_pose, const double base_link_to_front) +{ + return autoware_utils::calcOffsetPose(base_link_pose, base_link_to_front, 0.0, 0.0); +} + +template +boost::optional findCollision( + const T & points, const autoware_utils::LineString3d & line) +{ + for (size_t i = 0; i < points.size() - 1; ++i) { + // Create path segment + const auto & p_front = autoware_utils::getPoint(points.at(i)); + const auto & p_back = autoware_utils::getPoint(points.at(i + 1)); + const autoware_utils::LineString2d path_segment{{p_front.x, p_front.y}, {p_back.x, p_back.y}}; + + // Find intersection + std::vector collision_points; + bg::intersection(to_2d(line), path_segment, collision_points); + + // Ignore if no collision found + if (collision_points.empty()) { + continue; + } + + // Select first collision if found + const auto collision_point_2d = collision_points.front(); + + // Calculate interpolation ratio + const auto interpolate_ratio = + calcDistance2d(p_front, toMsg(collision_point_2d.to_3d(0))) / calcDistance2d(p_front, p_back); + + // Interpolate z + const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); + + // To point + const auto collision_point = + autoware_utils::createPoint(collision_point_2d.x(), collision_point_2d.y(), interpolated_z); + + return SegmentIndexWithPoint{i, collision_point}; + } + + return {}; +} + +template +boost::optional findCollision( + const T & points, const std::vector & lines) +{ + for (const auto & line : lines) { + const auto collision = findCollision(points, line); + if (collision) { + return collision; + } + } + + return {}; +} + +SegmentIndexWithOffset findForwardOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, + const double offset_length) +{ + double sum_length = 0.0; + for (size_t i = base_idx; i < path.points.size() - 1; ++i) { + const auto p_front = path.points.at(i); + const auto p_back = path.points.at(i + 1); + + const auto segment_length = calcDistance2d(p_front, p_back); + sum_length += segment_length; + + // If it's over offset length, return front index and offset from front point + if (sum_length >= offset_length) { + const auto remain_length = sum_length - offset_length; + return SegmentIndexWithOffset{i, segment_length - remain_length}; + } + } + + // No enough length + const auto last_segment_length = + calcDistance2d(path.points.at(path.points.size() - 2), path.points.at(path.points.size() - 1)); + + return {path.points.size() - 2, offset_length - sum_length + last_segment_length}; +} + +SegmentIndexWithOffset findBackwardOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx, + const double offset_length) +{ + double sum_length = 0.0; + for (size_t i = base_idx - 1; i > 0; --i) { + const auto p_front = path.points.at(i - 1); + const auto p_back = path.points.at(i); + + const auto segment_length = calcDistance2d(p_front, p_back); + sum_length += segment_length; + + // If it's over offset length, return front index and offset from front point + if (sum_length >= offset_length) { + const auto remain_length = sum_length - offset_length; + return SegmentIndexWithOffset{i - 1, remain_length}; + } + } + + // No enough length + return {0, sum_length - offset_length}; +} + +SegmentIndexWithOffset findOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset) +{ + if (offset >= 0) { + return findForwardOffsetSegment(path, index, offset); + } + + return findBackwardOffsetSegment(path, index, -offset); +} + +geometry_msgs::msg::Pose calcInterpolatedPose( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset) +{ + // Get segment points + const auto & p_front = path.points.at(index).point.pose.position; + const auto & p_back = path.points.at(index + 1).point.pose.position; + + // To Eigen point + const auto p_eigen_front = Eigen::Vector2d(p_front.x, p_front.y); + const auto p_eigen_back = Eigen::Vector2d(p_back.x, p_back.y); + + // Calculate interpolation ratio + const auto interpolate_ratio = offset / (p_eigen_back - p_eigen_front).norm(); + + // Add offset to front point + const auto interpolated_point_2d = + p_eigen_front + interpolate_ratio * (p_eigen_back - p_eigen_front); + const double interpolated_z = p_front.z + interpolate_ratio * (p_back.z - p_front.z); + + // Calculate orientation so that X-axis would be along the trajectory + tf2::Quaternion quat; + quat.setRPY(0, 0, autoware_utils::calcAzimuthAngle(p_front, p_back)); + + // To Pose + geometry_msgs::msg::Pose interpolated_pose; + interpolated_pose.position.x = interpolated_point_2d.x(); + interpolated_pose.position.y = interpolated_point_2d.y(); + interpolated_pose.position.z = interpolated_z; + interpolated_pose.orientation = tf2::toMsg(quat); + + return interpolated_pose; +} + +void insertStopVelocityFromStart(autoware_auto_planning_msgs::msg::PathWithLaneId * path) +{ + for (auto & p : path->points) { + p.point.longitudinal_velocity_mps = 0.0; + } +} + +size_t insertStopVelocityAtCollision( + const SegmentIndexWithPoint & collision, const double offset, + autoware_auto_planning_msgs::msg::PathWithLaneId * path) +{ + const auto collision_offset = + autoware_utils::calcLongitudinalOffsetToSegment(path->points, collision.index, collision.point); + + const auto offset_segment = findOffsetSegment(*path, collision.index, offset + collision_offset); + const auto interpolated_pose = + calcInterpolatedPose(*path, offset_segment.index, offset_segment.offset); + + if (offset_segment.offset < 0) { + insertStopVelocityFromStart(path); + return 0; + } + + const auto insert_index = offset_segment.index + 1; + auto insert_point = path->points.at(insert_index); + insert_point.point.pose = interpolated_pose; + + path->points.insert(path->points.begin() + insert_index, insert_point); + + // Insert 0 velocity after stop point + for (size_t i = insert_index; i < path->points.size(); ++i) { + path->points.at(i).point.longitudinal_velocity_mps = 0.0; + } + + return insert_index; +} +} // namespace + +VirtualTrafficLightModule::VirtualTrafficLightModule( + const int64_t module_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + module_id_(module_id), + reg_elem_(reg_elem), + lane_(lane), + planner_param_(planner_param) +{ + // Get map data + const auto instrument = reg_elem_.getVirtualTrafficLight(); + const auto instrument_bottom_line = toAutowarePoints(instrument); + + // Information from instrument + { + map_data_.instrument_type = *instrument.attribute("type").as(); + map_data_.instrument_id = std::to_string(instrument.id()); + map_data_.instrument_center = calcCenter(instrument_bottom_line); + } + + // Information from regulatory_element + { + map_data_.stop_line = toAutowarePoints(reg_elem_.getStopLine()); + map_data_.start_line = toAutowarePoints(reg_elem_.getStartLine()); + map_data_.end_lines = toAutowarePoints(reg_elem_.getEndLines()); + } + + // Custom tags + { + // Map attributes + for (const auto & attr : instrument.attributes()) { + const auto & key = attr.first; + const auto & value = *attr.second.as(); + + // Ignore mandatory tags + if (key == "type") { + continue; + } + + map_data_.custom_tags.emplace_back(createKeyValue(key, value)); + } + + // Lane ID + map_data_.custom_tags.emplace_back(createKeyValue("lane_id", std::to_string(lane_.id()))); + + // Turn direction + map_data_.custom_tags.emplace_back( + createKeyValue("turn_direction", lane_.attributeOr("turn_direction", "straight"))); + } + + // Set command + command_.type = map_data_.instrument_type; + command_.id = map_data_.instrument_id; + command_.custom_tags = map_data_.custom_tags; +} + +bool VirtualTrafficLightModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + // Initialize + setInfrastructureCommand({}); + *stop_reason = planning_utils::initializeStopReason( + autoware_planning_msgs::msg::StopReason::VIRTUAL_TRAFFIC_LIGHT); + module_data_ = {}; + + // Copy data + module_data_.head_pose = calcHeadPose( + planner_data_->current_pose.pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); + module_data_.path = *path; + + // Do nothing if vehicle is before start line + if (isBeforeStartLine()) { + RCLCPP_DEBUG(logger_, "before start_line"); + state_ = State::NONE; + updateInfrastructureCommand(); + return true; + } + + // Do nothing if vehicle is after any end line + if (isAfterAnyEndLine()) { + RCLCPP_DEBUG(logger_, "after end_line"); + state_ = State::FINALIZED; + updateInfrastructureCommand(); + return true; + } + + // Set state + state_ = State::REQUESTING; + + // Don't need to stop if there is no stop_line + if (!map_data_.stop_line) { + updateInfrastructureCommand(); + return true; + } + + // Find corresponding state + const auto virtual_traffic_light_state = findCorrespondingState(); + + // Stop at stop_line if no message received + if (!virtual_traffic_light_state) { + RCLCPP_DEBUG(logger_, "no message received"); + insertStopVelocityAtStopLine(path, stop_reason); + updateInfrastructureCommand(); + return true; + } + + // Stop at stop_line if no right is given + if (!hasRightOfWay(*virtual_traffic_light_state)) { + RCLCPP_DEBUG(logger_, "no right is given"); + insertStopVelocityAtStopLine(path, stop_reason); + updateInfrastructureCommand(); + return true; + } + + // Stop at stop_line if state is timeout before stop_line + if (isBeforeStopLine()) { + if (!isStateTimeout(*virtual_traffic_light_state)) { + RCLCPP_DEBUG(logger_, "state is timeout"); + insertStopVelocityAtStopLine(path, stop_reason); + } + + updateInfrastructureCommand(); + return true; + } + + // After stop_line + state_ = State::PASSING; + + // Stop at stop_line if finalization isn't completed + if (!virtual_traffic_light_state->is_finalized) { + RCLCPP_DEBUG(logger_, "finalization isn't completed"); + insertStopVelocityAtEndLine(path, stop_reason); + + if (isNearAnyEndLine() && planner_data_->isVehicleStopped()) { + state_ = State::FINALIZING; + } + } + + updateInfrastructureCommand(); + return true; +} + +void VirtualTrafficLightModule::updateInfrastructureCommand() +{ + command_.stamp = clock_->now(); + command_.state = static_cast(state_); + setInfrastructureCommand(command_); +} + +void VirtualTrafficLightModule::setStopReason( + const geometry_msgs::msg::Pose & stop_pose, autoware_planning_msgs::msg::StopReason * stop_reason) +{ + autoware_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.push_back(toMsg(map_data_.instrument_center)); + planning_utils::appendStopReason(stop_factor, stop_reason); +} + +bool VirtualTrafficLightModule::isBeforeStartLine() +{ + const auto collision = findCollision(module_data_.path.points, map_data_.start_line); + + // Since the module is registered, a collision should be detected usually. + // Therefore if no collision found, vehicle's path is fully after the line. + if (!collision) { + return false; + } + + const auto signed_arc_length = autoware_utils::calcSignedArcLength( + module_data_.path.points, module_data_.head_pose.position, collision->point); + + return signed_arc_length > 0; +} + +bool VirtualTrafficLightModule::isBeforeStopLine() +{ + const auto collision = findCollision(module_data_.path.points, *map_data_.stop_line); + + // Since the module is registered, a collision should be detected usually. + // Therefore if no collision found, vehicle's path is fully after the line. + if (!collision) { + return false; + } + + const auto signed_arc_length = autoware_utils::calcSignedArcLength( + module_data_.path.points, module_data_.head_pose.position, collision->point); + + return signed_arc_length > 0; +} + +bool VirtualTrafficLightModule::isAfterAnyEndLine() +{ + // Assume stop line is before end lines + if (isBeforeStopLine()) { + return false; + } + + const auto collision = findCollision(module_data_.path.points, map_data_.end_lines); + + // Since the module is registered, a collision should be detected usually. + // Therefore if no collision found, vehicle's path is fully after the line. + if (!collision) { + return false; + } + + const auto signed_arc_length = autoware_utils::calcSignedArcLength( + module_data_.path.points, module_data_.head_pose.position, collision->point); + + constexpr double max_excess_distance = 3.0; + return signed_arc_length < -max_excess_distance; +} + +bool VirtualTrafficLightModule::isNearAnyEndLine() +{ + const auto collision = findCollision(module_data_.path.points, map_data_.end_lines); + + if (!collision) { + return false; + } + + const auto signed_arc_length = autoware_utils::calcSignedArcLength( + module_data_.path.points, module_data_.head_pose.position, collision->point); + + constexpr double near_distance = 1.0; + return std::abs(signed_arc_length) < near_distance; +} + +boost::optional +VirtualTrafficLightModule::findCorrespondingState() +{ + // No message + if (!planner_data_->virtual_traffic_light_states) { + return {}; + } + + for (const auto & state : planner_data_->virtual_traffic_light_states->states) { + if (state.id == map_data_.instrument_id) { + return state; + } + } + + return {}; +} + +bool VirtualTrafficLightModule::isStateTimeout( + const autoware_v2x_msgs::msg::VirtualTrafficLightState & state) +{ + const auto delay = (clock_->now() - rclcpp::Time(state.stamp)).seconds(); + if (delay > planner_param_.max_delay_sec) { + RCLCPP_DEBUG(logger_, "delay=%f, max_delay=%f", delay, planner_param_.max_delay_sec); + return false; + } + + return true; +} + +bool VirtualTrafficLightModule::hasRightOfWay( + const autoware_v2x_msgs::msg::VirtualTrafficLightState & state) +{ + return state.approval; +} + +void VirtualTrafficLightModule::insertStopVelocityAtStopLine( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + const auto collision = findCollision(path->points, *map_data_.stop_line); + + geometry_msgs::msg::Pose stop_pose{}; + if (!collision) { + insertStopVelocityFromStart(path); + stop_pose = planner_data_->current_pose.pose; + } else { + const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path); + stop_pose = path->points.at(insert_index).point.pose; + } + + // Set StopReason + setStopReason(stop_pose, stop_reason); + + // Set data for visualization + module_data_.stop_head_pose_at_stop_line = + calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); +} + +void VirtualTrafficLightModule::insertStopVelocityAtEndLine( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + const auto collision = findCollision(path->points, map_data_.end_lines); + + geometry_msgs::msg::Pose stop_pose{}; + if (!collision) { + // No enough length + if (isBeforeStopLine()) { + return; + } + + insertStopVelocityFromStart(path); + stop_pose = planner_data_->current_pose.pose; + } else { + const auto offset = -planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto insert_index = insertStopVelocityAtCollision(*collision, offset, path); + stop_pose = path->points.at(insert_index).point.pose; + } + + // Set StopReason + setStopReason(stop_pose, stop_reason); + + // Set data for visualization + module_data_.stop_head_pose_at_end_line = + calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/utilization/interpolate.cpp b/planning/behavior_velocity_planner/src/utilization/interpolate.cpp new file mode 100644 index 0000000000000..c6b1932d2b41b --- /dev/null +++ b/planning/behavior_velocity_planner/src/utilization/interpolate.cpp @@ -0,0 +1,134 @@ +// Copyright 2018-2019 Autoware Foundation +// +// 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 +#include + +#include + +namespace behavior_velocity_planner +{ +namespace interpolation +{ +// TODO(murooka) delete these functions +/* + * linear interpolation + */ +bool LinearInterpolate::interpolate( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index, std::vector & return_value) +{ + if (!isValidInput(base_index, base_value, return_index)) { + std::cerr << "[interpolate] invalid input. interpolation failed." << std::endl; + return false; + } + + // calculate linear interpolation + int i = 0; + for (const auto idx : return_index) { + if (base_index[i] == idx) { + return_value.push_back(base_value[i]); + continue; + } + while (base_index[i] < idx) { + ++i; + } + if (i <= 0 || static_cast(base_index.size()) - 1 < i) { + std::cerr << "[interpolate] undesired condition. skip this idx!" << std::endl; + continue; + } + + const double base_dist = base_index[i] - base_index[i - 1]; + const double to_forward = base_index[i] - idx; + const double to_backward = idx - base_index[i - 1]; + if (to_forward < 0.0 || to_backward < 0.0) { + std::cerr << "[interpolate] undesired condition. skip this idx!!" << std::endl; + std::cerr << "i = " << i << ", base_index[i - 1] = " << base_index[i - 1] << ", idx = " << idx + << ", base_index[i] = " << base_index[i] << std::endl; + continue; + } + + const double value = (to_backward * base_value[i] + to_forward * base_value[i - 1]) / base_dist; + return_value.push_back(value); + } + return true; +} + +/* + * helper functions + */ +bool isIncrease(const std::vector & x) +{ + for (int i = 0; i < static_cast(x.size()) - 1; ++i) { + if (x[i] > x[i + 1]) { + return false; + } + } + return true; +} + +bool isValidInput( + const std::vector & base_index, const std::vector & base_value, + const std::vector & return_index) +{ + if (base_index.empty() || base_value.empty() || return_index.empty()) { + std::cout << "bad index : some vector is empty. base_index: " << base_index.size() + << ", base_value: " << base_value.size() << ", return_index: " << return_index.size() + << std::endl; + return false; + } + if (!isIncrease(base_index)) { + std::cout << "bad index : base_index is not monotonically increasing. base_index = [" + << base_index.front() << ", " << base_index.back() << "]" << std::endl; + return false; + } + if (!isIncrease(return_index)) { + std::cout << "bad index : base_index is not monotonically increasing. return_index = [" + << return_index.front() << ", " << return_index.back() << "]" << std::endl; + return false; + } + if (return_index.front() < base_index.front()) { + std::cout << "bad index : return_index.front() < base_index.front()" << std::endl; + return false; + } + if (base_index.back() < return_index.back()) { + std::cout << "bad index : base_index.back() < return_index.back()" << std::endl; + return false; + } + if (base_index.size() != base_value.size()) { + std::cout << "bad index : base_index.size() != base_value.size()" << std::endl; + return false; + } + + return true; +} + +std::vector calcEuclidDist(const std::vector & x, const std::vector & y) +{ + if (x.size() != y.size()) { + std::cerr << "x y vector size should be the same." << std::endl; + } + + std::vector dist_v; + dist_v.push_back(0.0); + for (unsigned int i = 0; i < x.size() - 1; ++i) { + const double dx = x.at(i + 1) - x.at(i); + const double dy = y.at(i + 1) - y.at(i); + dist_v.push_back(dist_v.at(i) + std::hypot(dx, dy)); + } + + return dist_v; +} +} // namespace interpolation +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp new file mode 100644 index 0000000000000..f824be8af89fe --- /dev/null +++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp @@ -0,0 +1,169 @@ +// Copyright 2020 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 +#include +#include + +#include +#include + +#include +#include +#include + +namespace behavior_velocity_planner +{ +autoware_auto_planning_msgs::msg::Path interpolatePath( + const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval) +{ + const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")}; + autoware_auto_planning_msgs::msg::Path interpolated_path; + + std::vector x, x_interp; + std::vector y, y_interp; + std::vector z, z_interp; + std::vector v, v_interp; + std::vector s_in, s_out; + if (2000 < path.points.size()) { + RCLCPP_WARN( + logger, "because path size is too large, calculation cost is high. size is %d.", + (int)path.points.size()); + } + if (path.points.size() < 2) { + RCLCPP_WARN(logger, "Do not interpolate because path size is 1."); + return path; + } + + // Calculate sample points + { + double s = 0.0; + for (size_t idx = 0; idx < path.points.size(); ++idx) { + const auto path_point = path.points.at(idx); + x.push_back(path_point.pose.position.x); + y.push_back(path_point.pose.position.y); + z.push_back(path_point.pose.position.z); + v.push_back(path_point.longitudinal_velocity_mps); + if (idx != 0) { + const auto path_point_prev = path.points.at(idx - 1); + s += autoware_utils::calcDistance3d(path_point_prev.pose, path_point.pose); + } + s_in.push_back(s); + } + } + + // Calculate query points + // Remove query point if query point is near input path point + const double epsilon = 0.01; + const double interpolation_interval = interval; + size_t checkpoint_idx = 1; + for (double s = interpolation_interval; s < std::min(length, s_in.back()); + s += interpolation_interval) { + while (checkpoint_idx < s_in.size() && s_in.at(checkpoint_idx) < s) { + s_out.push_back(s_in.at(checkpoint_idx)); + v_interp.push_back(v.at(checkpoint_idx)); + ++checkpoint_idx; + } + if ( + std::fabs(s - s_in.at(checkpoint_idx - 1)) > epsilon && + std::fabs(s - s_in.at(checkpoint_idx)) > epsilon) { + s_out.push_back(s); + v_interp.push_back(v.at(checkpoint_idx - 1)); + } + } + + if (s_out.empty()) { + RCLCPP_WARN(logger, "Do not interpolate because s_out is empty."); + return path; + } + + // Interpolate + x_interp = interpolation::slerp(s_in, x, s_out); + y_interp = interpolation::slerp(s_in, y, s_out); + z_interp = interpolation::slerp(s_in, z, s_out); + + // Insert boundary points + x_interp.insert(x_interp.begin(), x.front()); + y_interp.insert(y_interp.begin(), y.front()); + z_interp.insert(z_interp.begin(), z.front()); + v_interp.insert(v_interp.begin(), v.front()); + + x_interp.push_back(x.back()); + y_interp.push_back(y.back()); + z_interp.push_back(z.back()); + v_interp.push_back(v.back()); + + // Insert path point to interpolated_path + for (size_t idx = 0; idx < v_interp.size() - 1; ++idx) { + autoware_auto_planning_msgs::msg::PathPoint path_point; + path_point.pose.position.x = x_interp.at(idx); + path_point.pose.position.y = y_interp.at(idx); + path_point.pose.position.z = z_interp.at(idx); + path_point.longitudinal_velocity_mps = v_interp.at(idx); + const double yaw = + std::atan2(y_interp.at(idx + 1) - y_interp.at(idx), x_interp.at(idx + 1) - x_interp.at(idx)); + tf2::Quaternion quat; + quat.setRPY(0, 0, yaw); + path_point.pose.orientation = tf2::toMsg(quat); + interpolated_path.points.push_back(path_point); + } + interpolated_path.points.push_back(path.points.back()); + + return interpolated_path; +} + +autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_auto_planning_msgs::msg::Path & path) +{ + autoware_auto_planning_msgs::msg::Path filtered_path; + + const double epsilon = 0.01; + size_t latest_id = 0; + for (size_t i = 0; i < path.points.size(); ++i) { + double dist = 0.0; + if (i != 0) { + const double x = + path.points.at(i).pose.position.x - path.points.at(latest_id).pose.position.x; + const double y = + path.points.at(i).pose.position.y - path.points.at(latest_id).pose.position.y; + dist = std::sqrt(x * x + y * y); + } + if (i == 0 || epsilon < dist /*init*/) { + latest_id = i; + filtered_path.points.push_back(path.points.at(latest_id)); + } else { + filtered_path.points.back().longitudinal_velocity_mps = std::min( + filtered_path.points.back().longitudinal_velocity_mps, + path.points.at(i).longitudinal_velocity_mps); + } + } + + return filtered_path; +} +autoware_auto_planning_msgs::msg::Path filterStopPathPoint( + const autoware_auto_planning_msgs::msg::Path & path) +{ + autoware_auto_planning_msgs::msg::Path filtered_path = path; + bool found_stop = false; + for (size_t i = 0; i < filtered_path.points.size(); ++i) { + if (std::fabs(filtered_path.points.at(i).longitudinal_velocity_mps) < 0.01) { + found_stop = true; + } + if (found_stop) { + filtered_path.points.at(i).longitudinal_velocity_mps = 0.0; + } + } + return filtered_path; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp new file mode 100644 index 0000000000000..30569a26c341a --- /dev/null +++ b/planning/behavior_velocity_planner/src/utilization/util.cpp @@ -0,0 +1,300 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 + +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace planning_utils +{ +Polygon2d toFootprintPolygon(const autoware_auto_perception_msgs::msg::PredictedObject & object) +{ + Polygon2d obj_footprint; + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + obj_footprint = toBoostPoly(object.shape.footprint); + } else { + // cylinder type is treated as square-polygon + obj_footprint = + obj2polygon(object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions); + } + return obj_footprint; +} + +bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); + const bool is_target_ahead = (p.position.x > 0.0); + return is_target_ahead; +} + +Polygon2d generatePathPolygon( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const size_t end_idx, const double width) +{ + Polygon2d ego_area; // open polygon + for (size_t i = start_idx; i <= end_idx; ++i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x + width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y - width * std::cos(yaw); + ego_area.outer().push_back(Point2d(x, y)); + } + for (size_t i = end_idx; i >= start_idx; --i) { + const double yaw = tf2::getYaw(path.points.at(i).point.pose.orientation); + const double x = path.points.at(i).point.pose.position.x - width * std::sin(yaw); + const double y = path.points.at(i).point.pose.position.y + width * std::cos(yaw); + ego_area.outer().push_back(Point2d(x, y)); + } + return ego_area; +} + +double normalizeEulerAngle(double euler) +{ + double res = euler; + while (res > M_PI) { + res -= (2.0 * M_PI); + } + while (res < -M_PI) { + res += 2.0 * M_PI; + } + + return res; +} + +geometry_msgs::msg::Quaternion getQuaternionFromYaw(double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); +} + +template +bool calcClosestIndex( + const T & path, const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr, + double angle_thr) +{ + double dist_squared_min = std::numeric_limits::max(); + double yaw_pose = tf2::getYaw(pose.orientation); + closest = -1; + + for (int i = 0; i < static_cast(path.points.size()); ++i) { + const double dist_squared = calcSquaredDist2d(getPose(path, i), pose); + + /* check distance threshold */ + if (dist_squared > dist_thr * dist_thr) { + continue; + } + + /* check angle threshold */ + double yaw_i = tf2::getYaw(getPose(path, i).orientation); + double yaw_diff = normalizeEulerAngle(yaw_pose - yaw_i); + + if (std::fabs(yaw_diff) > angle_thr) { + continue; + } + + if (dist_squared < dist_squared_min) { + dist_squared_min = dist_squared; + closest = i; + } + } + + return closest == -1 ? false : true; +} + +template bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::Trajectory & path, const geometry_msgs::msg::Pose & pose, + int & closest, double dist_thr, double angle_thr); +template bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Pose & pose, int & closest, double dist_thr, double angle_thr); +template bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::Path & path, const geometry_msgs::msg::Pose & pose, + int & closest, double dist_thr, double angle_thr); + +template +bool calcClosestIndex( + const T & path, const geometry_msgs::msg::Point & point, int & closest, double dist_thr) +{ + double dist_squared_min = std::numeric_limits::max(); + closest = -1; + + for (int i = 0; i < static_cast(path.points.size()); ++i) { + const double dist_squared = calcSquaredDist2d(getPose(path, i), point); + + /* check distance threshold */ + if (dist_squared > dist_thr * dist_thr) { + continue; + } + + if (dist_squared < dist_squared_min) { + dist_squared_min = dist_squared; + closest = i; + } + } + + return closest == -1 ? false : true; +} +template bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::Trajectory & path, + const geometry_msgs::msg::Point & point, int & closest, double dist_thr); +template bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & point, int & closest, double dist_thr); +template bool calcClosestIndex( + const autoware_auto_planning_msgs::msg::Path & path, const geometry_msgs::msg::Point & point, + int & closest, double dist_thr); + +geometry_msgs::msg::Pose transformRelCoordinate2D( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + // translation + geometry_msgs::msg::Point trans_p; + trans_p.x = target.position.x - origin.position.x; + trans_p.y = target.position.y - origin.position.y; + + // rotation (use inverse matrix of rotation) + double yaw = tf2::getYaw(origin.orientation); + + geometry_msgs::msg::Pose res; + res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y); + res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); + res.position.z = target.position.z - origin.position.z; + res.orientation = getQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); + + return res; +} + +geometry_msgs::msg::Pose transformAbsCoordinate2D( + const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin) +{ + // rotation + geometry_msgs::msg::Point rot_p; + double yaw = tf2::getYaw(origin.orientation); + rot_p.x = (std::cos(yaw) * relative.position.x) + (-std::sin(yaw) * relative.position.y); + rot_p.y = (std::sin(yaw) * relative.position.x) + (std::cos(yaw) * relative.position.y); + + // translation + geometry_msgs::msg::Pose absolute; + absolute.position.x = rot_p.x + origin.position.x; + absolute.position.y = rot_p.y + origin.position.y; + absolute.position.z = relative.position.z + origin.position.z; + absolute.orientation = getQuaternionFromYaw(tf2::getYaw(relative.orientation) + yaw); + + return absolute; +} + +double calcJudgeLineDistWithAccLimit( + const double velocity, const double max_stop_acceleration, const double delay_response_time) +{ + double judge_line_dist = + (velocity * velocity) / (2.0 * (-max_stop_acceleration)) + delay_response_time * velocity; + return judge_line_dist; +} + +double calcJudgeLineDistWithJerkLimit( + const double velocity, const double acceleration, const double max_stop_acceleration, + const double max_stop_jerk, const double delay_response_time) +{ + if (velocity <= 0.0) { + return 0.0; + } + + /* t0: subscribe traffic light state and decide to stop */ + /* t1: braking start (with jerk limitation) */ + /* t2: reach max stop acceleration */ + /* t3: stop */ + + const double t1 = delay_response_time; + const double x1 = velocity * t1; + + const double v2 = velocity + (std::pow(max_stop_acceleration, 2) - std::pow(acceleration, 2)) / + (2.0 * max_stop_jerk); + + if (v2 <= 0.0) { + const double t2 = -1.0 * + (max_stop_acceleration + + std::sqrt(acceleration * acceleration - 2.0 * max_stop_jerk * velocity)) / + max_stop_jerk; + const double x2 = + velocity * t2 + acceleration * std::pow(t2, 2) / 2.0 + max_stop_jerk * std::pow(t2, 3) / 6.0; + return std::max(0.0, x1 + x2); + } + + const double t2 = (max_stop_acceleration - acceleration) / max_stop_jerk; + const double x2 = + velocity * t2 + acceleration * std::pow(t2, 2) / 2.0 + max_stop_jerk * std::pow(t2, 3) / 6.0; + + const double x3 = -1.0 * std::pow(v2, 2) / (2.0 * max_stop_acceleration); + return std::max(0.0, x1 + x2 + x3); +} + +autoware_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason) +{ + autoware_planning_msgs::msg::StopReason stop_reason_msg; + stop_reason_msg.reason = stop_reason; + return stop_reason_msg; +} + +void appendStopReason( + const autoware_planning_msgs::msg::StopFactor stop_factor, + autoware_planning_msgs::msg::StopReason * stop_reason) +{ + stop_reason->stop_factors.emplace_back(stop_factor); +} + +std::vector toRosPoints( + const autoware_auto_perception_msgs::msg::PredictedObjects & object) +{ + std::vector points; + for (const auto & obj : object.objects) { + points.emplace_back(obj.kinematics.initial_pose_with_covariance.pose.position); + } + return points; +} + +geometry_msgs::msg::Point toRosPoint(const pcl::PointXYZ & pcl_point) +{ + geometry_msgs::msg::Point point; + point.x = pcl_point.x; + point.y = pcl_point.y; + point.z = pcl_point.z; + return point; +} + +geometry_msgs::msg::Point toRosPoint(const Point2d & boost_point, const double z) +{ + geometry_msgs::msg::Point point; + point.x = boost_point.x(); + point.y = boost_point.y(); + point.z = z; + return point; +} + +LineString2d extendLine( + const lanelet::ConstPoint3d & lanelet_point1, const lanelet::ConstPoint3d & lanelet_point2, + const double & length) +{ + const Eigen::Vector2d p1(lanelet_point1.x(), lanelet_point1.y()); + const Eigen::Vector2d p2(lanelet_point2.x(), lanelet_point2.y()); + const Eigen::Vector2d t = (p2 - p1).normalized(); + return { + {(p1 - length * t).x(), (p1 - length * t).y()}, {(p2 + length * t).x(), (p2 + length * t).y()}}; +} +} // namespace planning_utils +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/stop-line-design.md b/planning/behavior_velocity_planner/stop-line-design.md new file mode 100644 index 0000000000000..05178b7d0550a --- /dev/null +++ b/planning/behavior_velocity_planner/stop-line-design.md @@ -0,0 +1,74 @@ +### Stop Line + +#### Role + +This module plans velocity so that the vehicle can stop right before stop lines and restart driving after stopped. + +#### Module Parameters + +| Parameter | Type | Description | +| ----------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | +| `stop_check_dist` | double | when the vehicle is within `stop_check_dist` from stop_line and stopped, move to STOPPED state | + +#### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +:find collision between path and stop_line; + +if (collision is found?) then (yes) +else (no) + stop +endif + +:find offset segment; + +:calculate stop pose; + +:calculate distance to stop line; + +if (state is APPROACH) then (yes) + :set stop velocity; + + if (vehicle is within stop_check_dist?) then (yes) + if (vehicle is stopped?) then (yes) + :change state to STOPPED; + endif + endif +else if (state is STOPPED) then (yes) + if (vehicle started to move?) then (yes) + :change state to START; + endif +else if (state is START) then (yes) + if ([optional] far from stop line?) then (yes) + :change state to APPROACH; + endif +endif + +stop +@enduml +``` + +This algorithm is based on `segment`. +`segment` consists of two node points. It's useful for removing boundary conditions because if `segment(i)` exists we can assume `node(i)` and `node(i+1)` exist. + +![node_and_segment](./docs/stop_line/node_and_segment.drawio.svg) + +First, this algorithm finds a collision between reference path and stop line. +Then, we can get `collision segment` and `collision point`. + +![find_collision_segment](./docs/stop_line/find_collision_segment.drawio.svg) + +Next, based on `collision point`, it finds `offset segment` by iterating backward points up to a specific offset length. +The offset length is `stop_margin`(parameter) + `base_link to front`(to adjust head pose to stop line). +Then, we can get `offset segment` and `offset from segment start`. + +![find_offset_segment](./docs/stop_line/find_offset_segment.drawio.svg) + +After that, we can calculate a offset point from `offset segment` and `offset`. This will be `stop_pose`. + +![calculate_stop_pose](./docs/stop_line/calculate_stop_pose.drawio.svg) diff --git a/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp new file mode 100644 index 0000000000000..d39b4ccb40d1b --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_arc_lane_util.cpp @@ -0,0 +1,106 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include + +using PathIndexWithPoint2d = behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; +using LineString2d = behavior_velocity_planner::LineString2d; +using Point2d = behavior_velocity_planner::Point2d; +namespace arc_lane_utils = behavior_velocity_planner::arc_lane_utils; + +TEST(findCollisionSegment, nominal) +{ + /** + * find forward collision segment by stop line + * s + * | = | = | = | s | = | + * 0 1 2 3 s 4 5 + * + **/ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + LineString2d stop_line; + stop_line.emplace_back(Point2d(3.5, 3.0)); + stop_line.emplace_back(Point2d(3.5, -3.0)); + auto segment = arc_lane_utils::findCollisionSegment(path, stop_line); + EXPECT_EQ(segment->first, static_cast(3)); + EXPECT_DOUBLE_EQ(segment->second.x(), 3.5); + EXPECT_DOUBLE_EQ(segment->second.y(), 0.0); +} + +TEST(findOffsetSegment, case_forward_offset_segment) +{ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + /** + * find offset segment forward + * | = | = | = | c | o | + * 0 1 2 3 4 5 + **/ + const int collision_segment_idx = 3; + Point2d collision_point(3.5, 0.0); + const PathIndexWithPoint2d & collision_segment = + std::make_pair(collision_segment_idx, collision_point); + // nominal + { + double offset_length = 1.0; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + const auto front_idx = offset_segment->first; + EXPECT_EQ(front_idx, static_cast(4)); + EXPECT_DOUBLE_EQ(offset_segment->second, 0.5); + } + // boundary condition + { + double offset_length = INFINITY; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + EXPECT_FALSE(offset_segment); + } +} + +TEST(findOffsetSegment, case_backward_offset_segment) +{ + auto path = test::generatePath(0.0, 0.0, 5.0, 0.0, 6); + /** + * find offset segment forward + * | = | = | o | c | = | + * 0 1 2 3 4 5 + **/ + const int collision_segment_idx = 3; + Point2d collision_point(3.5, 0.0); + const PathIndexWithPoint2d & collision_segment = + std::make_pair(collision_segment_idx, collision_point); + // nominal + { + double offset_length = -1.0; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + const auto front_idx = offset_segment->first; + EXPECT_EQ(front_idx, static_cast(2)); + EXPECT_DOUBLE_EQ(offset_segment->second, 0.5); + } + // boundary condition + { + double offset_length = -INFINITY; + const auto offset_segment = + arc_lane_utils::findOffsetSegment(path, collision_segment, offset_length); + EXPECT_FALSE(offset_segment); + } +} diff --git a/planning/behavior_velocity_planner/test/src/test_geometry.cpp b/planning/behavior_velocity_planner/test/src/test_geometry.cpp new file mode 100644 index 0000000000000..84a7477387c75 --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_geometry.cpp @@ -0,0 +1,209 @@ +// Copyright 2021 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 "utils.hpp" + +#include + +#include + +#include + +TEST(lerp, interpolation) +{ + using behavior_velocity_planner::geometry::lerp; + using V = Eigen::Vector2d; + // 1D + EXPECT_EQ(lerp(0.0, 1.0, 0.5), 0.5); + EXPECT_EQ(lerp(0.0, 1.0, 0.0), 0.0); + EXPECT_EQ(lerp(0.0, 1.0, 1.0), 1.0); + EXPECT_EQ(lerp(0.0, 1.0, 4.5), 4.5); + EXPECT_EQ(lerp(1.0, 2.0, 0.5), 1.5); + EXPECT_EQ(lerp(1.0, 2.0, 0.75), 1.75); + EXPECT_EQ(lerp(0.0, -2.0, 0.75), -1.5); + EXPECT_EQ(lerp(-10.0, -5.0, 0.5), -7.5); + // 2D + auto expect_eq = [](V v1, V v2) { + EXPECT_EQ(v1.x(), v2.x()); + EXPECT_EQ(v1.y(), v2.y()); + }; + expect_eq(lerp(V(0.0, 0.0), V(0.0, 1.0), 0.5), V(0.0, 0.5)); + expect_eq(lerp(V(0.0, 1.0), V(0.0, 2.0), 0.5), V(0.0, 1.5)); + expect_eq(lerp(V(0.0, 0.0), V(2.0, 2.0), 0.5), V(1.0, 1.0)); + expect_eq(lerp(V(-100.0, 10.0), V(0.0, 0.0), 0.1), V(-90.0, 9.0)); +} + +TEST(buildInterpolatedPolygon, straight_polygon) +{ + using behavior_velocity_planner::geometry::buildInterpolatedPolygon; + using V = Eigen::Vector2d; + auto expect_eq = [](V v1, V v2) { + EXPECT_EQ(v1.x(), v2.x()); + EXPECT_EQ(v1.y(), v2.y()); + }; + + lanelet::BasicLineString2d from; + lanelet::BasicLineString2d to; + double from_length; + double to_length; + double from_ratio_dist; + double to_ratio_dist; + lanelet::BasicPolygon2d polygon; + + from = {{0.0, 0.0}, {0.0, 10.0}}; + to = {{10.0, 0.0}, {10.0, 10.0}}; + from_length = 0.0; + to_length = 10.0; + from_ratio_dist = 0.0; + to_ratio_dist = 1.0; + buildInterpolatedPolygon( + polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); + ASSERT_EQ(polygon.size(), static_cast(4)); + expect_eq(polygon[0], from[0]); + expect_eq(polygon[1], from[1]); + expect_eq(polygon[2], to[1]); + expect_eq(polygon[3], to[0]); + + from_ratio_dist = 0.0; + to_ratio_dist = 0.5; + buildInterpolatedPolygon( + polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); + ASSERT_EQ(polygon.size(), static_cast(4)); + expect_eq(polygon[0], V(0.0, 0.0)); + expect_eq(polygon[1], V(0.0, 10.0)); + expect_eq(polygon[2], V(5.0, 10.0)); + expect_eq(polygon[3], V(5.0, 0.0)); + + from_ratio_dist = 0.25; + to_ratio_dist = 0.5; + buildInterpolatedPolygon( + polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); + ASSERT_EQ(polygon.size(), static_cast(4)); + expect_eq(polygon[0], V(2.5, 0.0)); + expect_eq(polygon[1], V(2.5, 10.0)); + expect_eq(polygon[2], V(5.0, 10.0)); + expect_eq(polygon[3], V(5.0, 0.0)); + + from_length = 1.5; + to_length = 7.5; + buildInterpolatedPolygon( + polygon, from, to, from_length, to_length, from_ratio_dist, to_ratio_dist); + ASSERT_EQ(polygon.size(), static_cast(4)); + expect_eq(polygon[0], V(2.5, 1.5)); + expect_eq(polygon[1], V(2.5, 7.5)); + expect_eq(polygon[2], V(5.0, 7.5)); + expect_eq(polygon[3], V(5.0, 1.5)); +} + +TEST(buildSlices, one_full_slice) +{ + using behavior_velocity_planner::geometry::buildSlices; + using behavior_velocity_planner::geometry::Slice; + using behavior_velocity_planner::geometry::SliceRange; + + /* Simple vertical lanelet + 3x3 slice on the left + 0 1 2 3 4 5 6 + 0 | |------ + 1 | | | + 2 | |SLICE| + 3 | |-----| + 4 | | + 5 | | + */ + const int nb_points = 6; + lanelet::ConstLanelet traj_lanelet = test::verticalLanelet({0.0, 0.0}, 1.0, 5.0, nb_points); + SliceRange range; + range.min_length = 0.0; + range.max_length = 3.0; + range.min_distance = 0.0; + range.max_distance = -3.0; + const double slice_length = 3.0; + const double slice_width = 3.0; + std::vector slices; + buildSlices(slices, traj_lanelet, range, slice_length, slice_width); + ASSERT_EQ(slices.size(), static_cast(1)); + EXPECT_EQ(slices[0].range.min_length, 0.0); + EXPECT_EQ(slices[0].range.min_distance, 0.0); + EXPECT_EQ(slices[0].range.max_length, 3.0); + EXPECT_EQ(slices[0].range.max_distance, -3.0); +} + +TEST(buildSlices, 3x3square_slice) +{ + using behavior_velocity_planner::geometry::buildSlices; + using behavior_velocity_planner::geometry::Slice; + using behavior_velocity_planner::geometry::SliceRange; + + /* Simple vertical lanelet + 3x3 slice on the left + 0 1 2 3 4 5 6 + 0 | |------ + 1 | | | + 2 | |SLICE| + 3 | |-----| + 4 | | + 5 | | + */ + const int nb_points = 6; + lanelet::ConstLanelet traj_lanelet = test::verticalLanelet({0.0, 0.0}, 1.0, 5.0, nb_points); + SliceRange range; + range.min_length = 0.0; + range.max_length = 3.0; + range.min_distance = 0.0; + range.max_distance = -3.0; + const double slice_length = 1.0; + const double slice_width = 1.0; + { + std::vector slices; + buildSlices(slices, traj_lanelet, range, slice_length, slice_width); + SliceRange ref; + auto equal_range = [&ref](const Slice & s) { + return s.range.min_distance == ref.min_distance && s.range.max_distance == ref.max_distance && + s.range.min_length == ref.min_length && s.range.max_length == ref.max_length; + }; + ASSERT_EQ(slices.size(), static_cast(9)); + for (double l = range.min_length; l < range.max_length; l += slice_length) { + for (double d = range.min_distance; d > range.max_distance; d -= slice_width) { + ref.min_length = l; + ref.max_length = l + slice_length; + ref.min_distance = d; + ref.max_distance = d - slice_width; + EXPECT_NE(std::find_if(slices.begin(), slices.end(), equal_range), slices.end()); + } + } + } + + // change to the left side (positive distance) + range.max_distance = 3.0; + { + std::vector slices; + buildSlices(slices, traj_lanelet, range, slice_length, slice_width); + SliceRange ref; + auto equal_range = [&ref](const Slice & s) { + return s.range.min_distance == ref.min_distance && s.range.max_distance == ref.max_distance && + s.range.min_length == ref.min_length && s.range.max_length == ref.max_length; + }; + ASSERT_EQ(slices.size(), static_cast(9)); + for (double l = range.min_length; l < range.max_length; l += slice_length) { + for (double d = range.min_distance; d < range.max_distance; d += slice_width) { + ref.min_length = l; + ref.max_length = l + slice_length; + ref.min_distance = d; + ref.max_distance = d + slice_width; + EXPECT_NE(std::find_if(slices.begin(), slices.end(), equal_range), slices.end()); + } + } + } +} diff --git a/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp new file mode 100644 index 0000000000000..3eac8b058463d --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_grid_utils.cpp @@ -0,0 +1,253 @@ +// Copyright 2021 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 "utils.hpp" + +#include + +#include + +#include + +struct indexHash +{ + std::size_t operator()(const grid_map::Index & i) const + { + return std::hash()(i.x()) ^ std::hash()(i.y()); + } +}; +struct indexEq +{ + bool operator()(const grid_map::Index & i1, const grid_map::Index & i2) const + { + return i1.x() == i2.x() && i1.y() == i2.y(); + } +}; + +using test::grid_param; + +TEST(isOcclusionSpotSquare, one_cell_occlusion_spot) +{ + using behavior_velocity_planner::grid_utils::isOcclusionSpotSquare; + using behavior_velocity_planner::grid_utils::OcclusionSpotSquare; + using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; + // Prepare an occupancy grid with a single occlusion_spot + /* + 0 1 2 3 + 0 ? ? ? + 1 ? ? + 2 ? ? ? + 3 ? ? + */ + grid_map::GridMap grid = test::generateGrid(4, 4, 1.0); + std::unordered_set unknown_cells = { + {0, 0}, {0, 3}, {1, 1}, {1, 2}, {2, 0}, {2, 1}, {2, 2}, {3, 0}, {3, 2}, {3, 3}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; + } + + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, 1, grid.getSize()); + if (unknown_cells.count({i, j})) { + ASSERT_TRUE(found); + } else { + ASSERT_FALSE(found); + } + } + } +} +TEST(isOcclusionSpotSquare, many_cells_occlusion_spot) +{ + using behavior_velocity_planner::grid_utils::isOcclusionSpotSquare; + using behavior_velocity_planner::grid_utils::OcclusionSpotSquare; + using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; + const double resolution = 1.0; + const double size = 2 * resolution; + /* + 0 1 2 3 + 0 ? ? ? + 1 ? ? + 2 ? ? ? + 3 ? ? + */ + grid_map::GridMap grid = test::generateGrid(4, 4, resolution); + std::unordered_set unknown_cells; + std::unordered_set occlusion_spot_cells; + unknown_cells = {{0, 0}, {0, 3}, {1, 1}, {1, 2}, {2, 0}, {2, 1}, {2, 2}, {3, 0}, {3, 2}, {3, 3}}; + occlusion_spot_cells = {{2, 0}, {3, 0}, {1, 2}, {3, 3}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; + } + + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = + isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); + if (occlusion_spot_cells.count({i, j})) { + ASSERT_TRUE(found); + } else { + ASSERT_FALSE(found); + } + } + } + /* + 0 1 2 3 + 0 ? + 1 ? ? + 2 ? ? ? + 3 + */ + grid = test::generateGrid(4, 4, 1.0); + unknown_cells = {{0, 2}, {1, 1}, {2, 0}, {2, 1}, {2, 2}, {3, 2}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; + } + + // No occlusion_spots + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = + isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); + ASSERT_FALSE(found); + } + } + /* + 0 1 2 3 + 0 + 1 ? ? + 2 ? ? + 3 + */ + grid = test::generateGrid(4, 4, 1.0); + unknown_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; + } + + // Only one occlusion_spot square + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = + isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); + if (i == 1 && j == 2) { + ASSERT_TRUE(found); + } else { + ASSERT_FALSE(found); + } + } + } + + /* + 0 1 2 3 + 0 ? + 1 ? ? + 2 ? ? ? ? + 3 ? + */ + grid = test::generateGrid(4, 4, 1.0); + unknown_cells = {{0, 2}, {1, 0}, {1, 1}, {1, 2}, {1, 3}, {2, 1}, {2, 2}, {3, 2}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; + } + occlusion_spot_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}}; + + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = + isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); + if (i == 1 && j == 2) { + ASSERT_TRUE(found); + } else { + ASSERT_FALSE(found); + } + } + } + + std::cout << "TooNoisyCase->No OcclusionSpot 2by2" << std::endl + << " 0|1|2|3|4| " << std::endl + << " 0 |?| | | | " << std::endl + << " 1 |?|?|?| | " << std::endl + << " 2 ?|?| |?| | " << std::endl + << " 3 |?| | | | " << std::endl + << " 4 |?| | | | " << std::endl; + grid = test::generateGrid(5, 5, 1.0); + unknown_cells = {{0, 2}, {1, 0}, {1, 1}, {1, 2}, {1, 3}, {2, 1}, {3, 1}, {3, 2}}; + for (grid_map::Index index : unknown_cells) { + grid.at("layer", index) = UNKNOWN; + } + occlusion_spot_cells = {{1, 1}, {1, 2}, {2, 1}, {2, 2}}; + + for (int i = 0; i < grid.getLength().x(); ++i) { + for (int j = 0; j < grid.getLength().y(); ++j) { + OcclusionSpotSquare occlusion_spot; + bool found = + isOcclusionSpotSquare(occlusion_spot, grid["layer"], {i, j}, size, grid.getSize()); + if (found) { + std::cout << "i: " << i << " j: " << j << " change algorithm or update test" << std::endl; + } + ASSERT_FALSE(found); + } + } +} + +TEST(buildSlices, test_buffer_offset) +{ + using behavior_velocity_planner::geometry::buildSlices; + using behavior_velocity_planner::geometry::Slice; + using behavior_velocity_planner::geometry::SliceRange; + + // Curving lanelet + // lanelet::Lanelet l; + // lanelet::Points3d line; + boost::geometry::model::linestring line; + boost::geometry::model::multi_polygon> + poly; + line.emplace_back(0.0, 1.0); + line.emplace_back(1.0, 4.0); + line.emplace_back(2.0, 6.0); + line.emplace_back(3.0, 7.0); + line.emplace_back(4.0, 6.0); + line.emplace_back(5.0, 4.0); + line.emplace_back(6.0, 1.0); + + boost::geometry::strategy::buffer::distance_asymmetric distance_strategy(0.0, 1.1); + boost::geometry::strategy::buffer::end_flat end_strategy; + boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::strategy::buffer::join_miter join_strategy; + boost::geometry::strategy::buffer::point_square point_strategy; + boost::geometry::buffer( + line, poly, distance_strategy, side_strategy, join_strategy, end_strategy, point_strategy); + + std::cout << boost::geometry::wkt(line) << "\n"; + std::cout << boost::geometry::wkt(poly[0]) << "\n"; + + boost::geometry::model::multi_point in1; + for (const auto & p : line) { + in1.push_back(p); + } + boost::geometry::model::multi_point in2; + for (const auto & p : poly[0].outer()) { + in2.push_back(p); + } + boost::geometry::model::multi_point output; + boost::geometry::difference(in2, in1, output); + + std::cout << boost::geometry::wkt(output) << "\n"; +} diff --git a/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp new file mode 100644 index 0000000000000..500784db5482d --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_occlusion_spot_utils.cpp @@ -0,0 +1,216 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include +#include + +autoware_auto_planning_msgs::msg::Path toPath( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) +{ + autoware_auto_planning_msgs::msg::Path path; + for (const auto & p : path_with_id.points) { + path.points.push_back(p.point); + } + return path; +} + +TEST(spline, splineInterpolate) +{ + using std::chrono::duration; + using std::chrono::duration_cast; + using std::chrono::high_resolution_clock; + using std::chrono::microseconds; + autoware_auto_planning_msgs::msg::PathWithLaneId path = test::generatePath(0, 0.0, 6.0, 0.0, 7); + const auto path_interp = behavior_velocity_planner::interpolatePath(toPath(path), 100, 0.5); + for (const auto & p : path_interp.points) { + std::cout << "interp" << p.pose.position.x << std::endl; + } + ASSERT_EQ(path_interp.points.size(), path.points.size() * 2 - +1); +} + +TEST(buildPathLanelet, nominal) +{ + using behavior_velocity_planner::occlusion_spot_utils::buildPathLanelet; + lanelet::ConstLanelet path_lanelet; + /* straight diagonal path + 0 1 2 3 4 + 0 x + 1 x + 2 x + 3 x + 4 x + */ + autoware_auto_planning_msgs::msg::PathWithLaneId path = test::generatePath(0, 0, 4, 4, 5); + path_lanelet = buildPathLanelet(path); + ASSERT_EQ(path_lanelet.centerline2d().front().x(), 0.0); + ASSERT_EQ(path_lanelet.centerline2d().front().y(), 0.0); + ASSERT_NE(path_lanelet.centerline2d().back().x(), 4.0); + ASSERT_NE(path_lanelet.centerline2d().back().y(), 4.0); + std::cout << "path lanelet size: " << path_lanelet.centerline2d().size() << std::endl; +} + +TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) +{ + using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; + using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using std::chrono::duration; + using std::chrono::duration_cast; + using std::chrono::high_resolution_clock; + using std::chrono::microseconds; + std::vector possible_collisions; + // make a path with 2000 points from x=0 to x=4 + autoware_auto_planning_msgs::msg::PathWithLaneId path = + test::generatePath(0.0, 3.0, 4.0, 3.0, 2000); + // make 2000 possible collision from x=0 to x=10 + test::generatePossibleCollisions(possible_collisions, 0.0, 3.0, 4.0, 3.0, 2000); + + /** + * @brief too many possible collisions on path + * + * Ego -col-col-col-col-col-col-col--col-col-col-col-col-col-col-col-col-> path + * + */ + + auto start_naive = high_resolution_clock::now(); + calcSlowDownPointsForPossibleCollision(0, path, 0, possible_collisions); + + auto end_naive = high_resolution_clock::now(); + // 2000 path * 2000 possible collisions + EXPECT_EQ(possible_collisions.size(), size_t{2000}); + EXPECT_EQ(path.points.size(), size_t{2000}); + EXPECT_TRUE(duration_cast(end_naive - start_naive).count() < 2000); + std::cout << " runtime (microsec) " + << duration_cast(end_naive - start_naive).count() << std::endl; +} + +TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) +{ + using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; + using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using std::chrono::duration; + using std::chrono::duration_cast; + using std::chrono::high_resolution_clock; + using std::chrono::microseconds; + std::vector possible_collisions; + const double offset = 2; + autoware_auto_planning_msgs::msg::PathWithLaneId path = + test::generatePath(offset, 3.0, 6.0, 3.0, 5); + test::generatePossibleCollisions(possible_collisions, 3.0, 3.0, 6.0, 3.0, 3); + for (size_t i = 0; i < path.points.size(); i++) { + path.points[i].point.longitudinal_velocity_mps = static_cast(i + offset); + } + + /** + * @brief generated path and possible collisions : path start from 2 to 6 + * 0 1 2 3 4 5 6 + * x: e-n-n-p-p-p-p-> path + * v: N-N-2-3-4-5-6-> velocity + * c: N-N-N-c-NcN-c-> collision + * --->| longitudinal offset + * e : ego + * p : path + * c : collision + */ + + calcSlowDownPointsForPossibleCollision(0, path, offset, possible_collisions); + EXPECT_EQ(possible_collisions[0].collision_path_point.longitudinal_velocity_mps, 3); + EXPECT_EQ(possible_collisions[1].collision_path_point.longitudinal_velocity_mps, 4.5); + EXPECT_EQ(possible_collisions[2].collision_path_point.longitudinal_velocity_mps, 6); +} + +TEST(createPossibleCollisionBehindParkedVehicle, TooManyPathPointsAndObstacles) +{ + using behavior_velocity_planner::occlusion_spot_utils::createPossibleCollisionBehindParkedVehicle; + using std::chrono::duration; + using std::chrono::duration_cast; + using std::chrono::high_resolution_clock; + using std::chrono::microseconds; + + // make a path with 200 points from x=0 to x=200 + autoware_auto_planning_msgs::msg::PathWithLaneId path = + test::generatePath(0.0, 3.0, 200.0, 3.0, 100); + // There is a parked bus,car,truck along with ego path. + // Ignore vehicle dimensions to simplify test + std::cout << " 6 - |TRU| | | -> truck is ignored because of lateral distance \n" + << " 5 - | | | | \n" + << " 4 - |CAR| | | -> considered \n" + << " 3Ego--|---|--path--> (2000 points) \n" + << " ＝＝median strip==== \n" + << " 2 - | | |SUB| -> bus is ignored because of opposite direction \n" + << " 1 - | | | | \n" + << " 0 | 1 | 2 | 3 | 4 | \n"; + + autoware_auto_perception_msgs::msg::PredictedObjects obj_arr; + autoware_auto_perception_msgs::msg::PredictedObject obj; + obj.shape.dimensions.x = 0.0; + obj.shape.dimensions.y = 0.0; + obj.kinematics.initial_pose_with_covariance.pose.orientation = + autoware_utils::createQuaternionFromYaw(0.0); + obj.kinematics.initial_twist_with_covariance.twist.linear.x = 0; + obj.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); + + // car + obj.kinematics.initial_pose_with_covariance.pose.position.x = 2.5; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 4.0; + obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + const size_t num_car = 30; + for (size_t i = 0; i < num_car; i++) { + obj_arr.objects.emplace_back(obj); + } + + // truck + obj.kinematics.initial_pose_with_covariance.pose.position.x = 2.5; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 6.0; + obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; + obj_arr.objects.emplace_back(obj); + + // bus + obj.kinematics.initial_pose_with_covariance.pose.position.x = 4.5; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 2.0; + obj.kinematics.initial_pose_with_covariance.pose.orientation = + autoware_utils::createQuaternionFromYaw(M_PI); + obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::BUS; + obj_arr.objects.emplace_back(obj); + + // Set parameters: enable sidewalk obstacles + behavior_velocity_planner::occlusion_spot_utils::PlannerParam parameters; + parameters.vehicle_info.baselink_to_front = 0.0; + parameters.vehicle_info.vehicle_width = 0.0; + parameters.detection_area_length = 100; + parameters.pedestrian_vel = 1.6; + parameters.lateral_distance_thr = 2.5; + parameters.angle_thr = 2.5; + + auto obj_arr_ptr = + std::make_shared(obj_arr); + auto start_naive = high_resolution_clock::now(); + std::vector + possible_collisions; + createPossibleCollisionBehindParkedVehicle(possible_collisions, path, parameters, 0, obj_arr_ptr); + auto end_naive = high_resolution_clock::now(); + // the possible collision is inserted and + // it's ego distance and obstacle distance is the same as (x,|y|) + ASSERT_EQ(possible_collisions.size(), static_cast(num_car)); + EXPECT_EQ(possible_collisions[0].arc_lane_dist_at_collision.length, 2.5); + EXPECT_EQ(possible_collisions[0].arc_lane_dist_at_collision.distance, 1); + std::cout << " runtime (microsec) " + << duration_cast(end_naive - start_naive).count() << std::endl; +} diff --git a/planning/behavior_velocity_planner/test/src/test_performances.cpp b/planning/behavior_velocity_planner/test/src/test_performances.cpp new file mode 100644 index 0000000000000..7a19116889bc3 --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_performances.cpp @@ -0,0 +1,84 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +using test::grid_param; + +TEST(performances, many_sidewalk_occlusion_spots) +{ + using std::chrono::duration; + using std::chrono::duration_cast; + using std::chrono::high_resolution_clock; + using std::chrono::microseconds; + + using behavior_velocity_planner::occlusion_spot_utils::generatePossibleCollisions; + using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + + std::vector possible_collisions; + + autoware_auto_planning_msgs::msg::PathWithLaneId trajectory = + test::generatePath(0.5, 0.5, 300.0, 0.5, 3000); + grid_map::GridMap grid = test::generateGrid(3000, 3000, 0.1); + for (int x = 0; x < grid.getSize().x(); ++x) { + for (int y = 0; y < grid.getSize().x(); ++y) { + grid.at("layer", grid_map::Index(x, y)) = + behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; + } + } + + // empty lanelet map + lanelet::LaneletMapPtr map = std::make_shared(); + + // Make routing graph + lanelet::traffic_rules::TrafficRulesPtr traffic_rules{ + lanelet::traffic_rules::TrafficRulesFactory::create( + lanelet::Locations::Germany, lanelet::Participants::Vehicle, + lanelet::traffic_rules::TrafficRules::Configuration())}; + lanelet::routing::RoutingCostPtrs cost_ptrs{ + std::make_shared( + lanelet::routing::RoutingCostDistance{2.})}; + lanelet::routing::RoutingGraphPtr routing_graph = + lanelet::routing::RoutingGraph::build(*map, *traffic_rules, cost_ptrs); + + // Set parameters: enable sidewalk obstacles + behavior_velocity_planner::occlusion_spot_utils::PlannerParam parameters; + parameters.vehicle_info.baselink_to_front = 0.0; + parameters.vehicle_info.vehicle_width = 1.0; + parameters.sidewalk.min_occlusion_spot_size = 1.0; + parameters.sidewalk.focus_range = 1.0; + parameters.grid = grid_param; + const double offset_from_ego_to_closest = 0; + const double offset_from_closest_to_target = 0; + std::vector to_remove; + auto start_closest = high_resolution_clock::now(); + generatePossibleCollisions( + possible_collisions, trajectory, grid, offset_from_ego_to_closest, + offset_from_closest_to_target, parameters, to_remove); + auto end_closest = high_resolution_clock::now(); + std::string ms = " (micro seconds) "; + std::cout << "| only_closest : runtime (microseconds) \n" + << "| true : " << duration_cast(end_closest - start_closest).count() << ms + << "\n"; +} diff --git a/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp new file mode 100644 index 0000000000000..5d19e30df0bf4 --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_risk_predictive_braking.cpp @@ -0,0 +1,113 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include + +TEST(calculatePredictiveBrakingVelocity, min_max) +{ + using behavior_velocity_planner::occlusion_spot_utils::calculatePredictiveBrakingVelocity; + const double inf = std::numeric_limits::max(); + std::cout << "PBS(v0,dist,pbs) of (10,2,-inf) --> 0[m/s] \n"; + // lower bound ego_vel + ASSERT_EQ(calculatePredictiveBrakingVelocity(0, 2, -inf), 0); + std::cout << "PBS(v0,dist,pbs) of (10,inf,0) --> 10[m/s] \n"; + // upper bound + ASSERT_EQ(calculatePredictiveBrakingVelocity(10, inf, 0), 10); +} + +TEST(calculateSafeRPBVelocity, min_max) +{ + using behavior_velocity_planner::occlusion_spot_utils::calculateSafeRPBVelocity; + // lower bound ttc_vir = 0 + const double t_buff = 0.5; + double d_obs = 0.5; + double v_obs = 1.0; + ASSERT_EQ(calculateSafeRPBVelocity(t_buff, d_obs, v_obs, -5.0), 0.0); + // lower bound ebs_decel = 0 + ASSERT_EQ(calculateSafeRPBVelocity(1.0, 0.5, 0.5, 0), 0.0); +} + +TEST(getPBSLimitedRPBVelocity, min_max) +{ + using behavior_velocity_planner::occlusion_spot_utils::getPBSLimitedRPBVelocity; + const double inf = std::numeric_limits::max(); + // upper bound rpb_vel + ASSERT_EQ(getPBSLimitedRPBVelocity(inf, inf, inf, inf), inf); + // lower bound org_vel = 0 + ASSERT_EQ(getPBSLimitedRPBVelocity(inf, inf, inf, 0), 0.0); + // lower bound min = 0 + ASSERT_EQ(getPBSLimitedRPBVelocity(inf, inf, 0, inf), inf); +} + +TEST(insertSafeVelocityToPath, replace_original_at_too_close_case) +{ + /* straight path + 0 1 2 3 4 + 0 x x z x x + ---> + straight path + 0 1 2 3 4 + 0 x x r xox + */ + using behavior_velocity_planner::occlusion_spot_utils::insertSafeVelocityToPath; + const int num_path_point = 5; + const double inf = std::numeric_limits::max(); + behavior_velocity_planner::occlusion_spot_utils::PlannerParam param; + param.angle_thr = 1.0; + param.dist_thr = 10.0; + autoware_auto_planning_msgs::msg::PathWithLaneId path = + test::generatePath(0.0, 0.0, static_cast(num_path_point - 1), 0.0, num_path_point); + geometry_msgs::msg::Pose pose{}; + pose.position.x = 2; + double safe_vel = inf; + // insert x=2 new point -> replace original + insertSafeVelocityToPath(pose, safe_vel, param, &path); + ASSERT_EQ(path.points.size(), static_cast(num_path_point)); + pose.position.x = 3.4; + insertSafeVelocityToPath(pose, safe_vel, param, &path); + ASSERT_EQ(path.points.size(), static_cast(num_path_point + 1)); +} + +TEST(insertSafeVelocityToPath, dont_insert_last_point) +{ + /* straight path + 0 1 2 3 4 + 0 x x z x xo + ---> + straight path + 0 1 2 3 4 + 0 x x x x x + */ + using behavior_velocity_planner::occlusion_spot_utils::insertSafeVelocityToPath; + const int num_path = 5; + geometry_msgs::msg::Pose pose{}; + pose.position.x = 5; + pose.position.y = 0; + pose.position.z = 0; + double safe_vel = 10; + behavior_velocity_planner::occlusion_spot_utils::PlannerParam param; + param.angle_thr = 1.0; + param.dist_thr = 10.0; + autoware_auto_planning_msgs::msg::PathWithLaneId path = + test::generatePath(0.0, 0.0, static_cast(num_path - 1), 0.0, num_path); + ASSERT_EQ(insertSafeVelocityToPath(pose, safe_vel, param, &path), -1); + ASSERT_EQ(path.points.size(), static_cast(num_path)); +} diff --git a/planning/behavior_velocity_planner/test/src/test_state_machine.cpp b/planning/behavior_velocity_planner/test/src/test_state_machine.cpp new file mode 100644 index 0000000000000..f156b3720c5ce --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_state_machine.cpp @@ -0,0 +1,74 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +#include +#include + +using StateMachine = behavior_velocity_planner::StateMachine; +using State = behavior_velocity_planner::StateMachine::State; + +int enumToInt(State s) { return static_cast(s); } + +TEST(state_machine, on_initialized) +{ + StateMachine state_machine = StateMachine(); + EXPECT_EQ(enumToInt(State::GO), enumToInt(state_machine.getState())); +} + +TEST(state_machine, set_state_stop) +{ + StateMachine state_machine = StateMachine(); + state_machine.setState(State::STOP); + EXPECT_EQ(enumToInt(State::STOP), enumToInt(state_machine.getState())); +} + +TEST(state_machine, set_state_stop_with_margin_time) +{ + StateMachine state_machine = StateMachine(); + const double margin_time = 1.0; + state_machine.setMarginTime(margin_time); + rclcpp::Clock current_time = rclcpp::Clock(RCL_ROS_TIME); + rclcpp::Logger logger = rclcpp::get_logger("test_set_state_with_margin_time"); + // DO NOT SET GO until margin time past + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::GO)); + state_machine.setStateWithMarginTime(State::STOP, logger, current_time); + // set STOP immediately when stop is set + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::STOP)); +} + +TEST(state_machine, set_state_go_with_margin_time) +{ + StateMachine state_machine = StateMachine(); + const double margin_time = 0.2; + state_machine.setMarginTime(margin_time); + rclcpp::Logger logger = rclcpp::get_logger("test_set_state_with_margin_time"); + state_machine.setState(State::STOP); + // loop until state change from STOP -> GO + while (state_machine.getState() == State::STOP) { + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::STOP)); + rclcpp::Clock current_time = rclcpp::Clock(RCL_ROS_TIME); + EXPECT_TRUE(state_machine.getDuration() < margin_time); + state_machine.setStateWithMarginTime(State::GO, logger, current_time); + } + // time past STOP -> GO + EXPECT_TRUE(state_machine.getDuration() > margin_time); + EXPECT_EQ(enumToInt(state_machine.getState()), enumToInt(State::GO)); +} diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/test/src/test_utilization.cpp new file mode 100644 index 0000000000000..4d16c285d0564 --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/test_utilization.cpp @@ -0,0 +1,40 @@ +// Copyright 2021 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 "utils.hpp" + +#include +#include + +#include + +TEST(to_footprint_polygon, nominal) +{ + using behavior_velocity_planner::planning_utils::toFootprintPolygon; + autoware_auto_perception_msgs::msg::PredictedObject obj = test::generatePredictedObject(0.0); + auto poly = toFootprintPolygon(obj); + EXPECT_TRUE(true); +} + +TEST(is_ahead_of, nominal) +{ + using behavior_velocity_planner::planning_utils::isAheadOf; + geometry_msgs::msg::Pose target = test::generatePose(0); + geometry_msgs::msg::Pose origin = test::generatePose(1); + bool is_ahead = isAheadOf(target, origin); + EXPECT_FALSE(is_ahead); + target = test::generatePose(2); + is_ahead = isAheadOf(target, origin); + EXPECT_TRUE(is_ahead); +} diff --git a/planning/behavior_velocity_planner/test/src/utils.hpp b/planning/behavior_velocity_planner/test/src/utils.hpp new file mode 100644 index 0000000000000..7afaff9d4a755 --- /dev/null +++ b/planning/behavior_velocity_planner/test/src/utils.hpp @@ -0,0 +1,198 @@ +// Copyright 2021 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 UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace test +{ +// Default grid parameter such that UNKNOWN cells have a value of 50 +const behavior_velocity_planner::grid_utils::GridParam grid_param = {10, 51}; + +/* Horizontal lanelet + 0 1 2 3 4 5 6 + 0 x x x x x x x + 1 + 2 x x x x x x x + 3 + 4 + 5 + */ +inline lanelet::ConstLanelet horizontalLanelet( + std::pair origin = {0, 0}, double width = 2.0, double length = 6.0, + int nb_points = 2) +{ + lanelet::Lanelet l; + lanelet::Points3d line; + for (double x = origin.first; x <= origin.first + length; x += length / (nb_points - 1)) { + line.emplace_back(lanelet::ConstPoint2d(0, x, origin.second)); + } + l.setLeftBound(lanelet::LineString3d(0, line)); + line.clear(); + for (double x = origin.second; x <= origin.first + length; x += length / (nb_points - 1)) { + line.emplace_back(lanelet::ConstPoint2d(0, x, origin.second + width)); + } + l.setRightBound(lanelet::LineString3d(1, line)); + return lanelet::ConstLanelet(l); +} +/* Vertical lanelet + 0 1 2 3 4 5 6 + 0 x x + 1 x x + 2 x x + 3 x x + 4 x x + 5 x x + */ +inline lanelet::ConstLanelet verticalLanelet( + std::pair origin = {0, 0}, double width = 2.0, double length = 5.0, + int nb_points = 2) +{ + lanelet::Lanelet l; + lanelet::Points3d line; + for (double y = origin.second; y <= origin.second + length; y += length / (nb_points - 1)) { + line.emplace_back(lanelet::ConstPoint2d(0, origin.first, y)); + } + l.setLeftBound(lanelet::LineString3d(0, line)); + line.clear(); + for (double y = origin.second; y <= origin.second + length; y += length / (nb_points - 1)) { + line.emplace_back(lanelet::ConstPoint2d(0, origin.first + width, y)); + } + l.setRightBound(lanelet::LineString3d(1, line)); + return lanelet::ConstLanelet(l); +} + +// /!\ columns and rows in the GridMap are "inverted" (x -> rows, y -> columns) +inline grid_map::GridMap generateGrid(int w, int h, double res) +{ + grid_map::GridMap grid{}; + grid_map::Length length(w * res, h * res); + grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); + grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); + return grid; +} + +inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( + double x0, double y0, double x, double y, int nb_points) +{ + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + double x_step = (x - x0) / (nb_points - 1); + double y_step = (y - y0) / (nb_points - 1); + for (int i = 0; i < nb_points; ++i) { + autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + point.point.pose.position.x = x0 + x_step * i; + point.point.pose.position.y = y0 + y_step * i; + point.point.pose.position.z = 0.0; + path.points.push_back(point); + } + return path; +} + +using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; +inline void generatePossibleCollisions( + std::vector & possible_collisions, double x0, double y0, double x, + double y, int nb_cols) +{ + using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; + using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + const double lon = 0.0; // assume col_x = intersection_x + const double lat = -1.0; + const double velocity = 1.0; + /** + * @brief representation of a possible collision between ego and some obstacle + * ^ + * | + * Ego ---------collision----------intersection-------> path + * | + * ------------------ | + * | Vehicle | obstacle + * ------------------ + */ + double x_step = (x - x0) / (nb_cols - 1); + double y_step = (y - y0) / (nb_cols - 1); + for (int i = 0; i < nb_cols; ++i) { + // collision + ObstacleInfo obstacle_info; + obstacle_info.position.x = x0 + x_step * i; + obstacle_info.position.y = y0 + y_step * i + lat; + obstacle_info.max_velocity = velocity; + + // intersection + geometry_msgs::msg::Pose intersection_pose{}; + intersection_pose.position.x = x0 + x_step * i + lon; + intersection_pose.position.x = y0 + y_step * i; + + // collision path point + autoware_auto_planning_msgs::msg::PathPoint collision_path_point{}; + collision_path_point.pose.position.x = x0 + x_step * i + lon; + collision_path_point.pose.position.y = y0 + y_step * i; + + lanelet::ArcCoordinates arc; + arc.length = obstacle_info.position.x; + arc.distance = obstacle_info.position.y; + + PossibleCollisionInfo col(obstacle_info, collision_path_point, intersection_pose, arc); + possible_collisions.emplace_back(col); + } +} +inline void addConstantVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId & trajectory, double velocity) +{ + for (auto & p : trajectory.points) { + p.point.longitudinal_velocity_mps = velocity; + } +} +inline autoware_auto_perception_msgs::msg::PredictedObject generatePredictedObject(double x) +{ + autoware_auto_perception_msgs::msg::PredictedObject obj; + obj.shape.dimensions.x = 5.0; + obj.shape.dimensions.y = 2.0; + tf2::Quaternion q; + obj.kinematics.initial_pose_with_covariance.pose.orientation = tf2::toMsg(q); + obj.kinematics.initial_twist_with_covariance.twist.linear.x = 0; + obj.kinematics.initial_pose_with_covariance.pose.position.x = x; + obj.kinematics.initial_pose_with_covariance.pose.position.y = 0; + obj.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); + obj.classification.at(0).label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + return obj; +} +inline geometry_msgs::msg::Pose generatePose(double x) +{ + geometry_msgs::msg::Pose p; + p.position.x = x; + p.position.y = 0.0; + p.position.z = 1.0; + tf2::Quaternion q; + q.setRPY(0, 0, 0); + p.orientation = tf2::toMsg(q); + return p; +} + +} // namespace test + +#endif // UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/traffic-light-design.md b/planning/behavior_velocity_planner/traffic-light-design.md new file mode 100644 index 0000000000000..dfcbed7eeb1ef --- /dev/null +++ b/planning/behavior_velocity_planner/traffic-light-design.md @@ -0,0 +1,120 @@ +### Traffic Light + +#### Role + +Judgement whether a vehicle can go into an intersection or not by internal and external traffic light status, and planning a velocity of the stop if necessary. +This module is designed for rule-based velocity decision that is easy for developers to design its behavior. It generates proper velocity for traffic light scene. + +In addition, the STOP/GO interface of behavior_velocity_planner allows external users / modules (e.g. remote operation) to intervene the decision of the internal perception. This function is expected to be used, for example, for remote intervention in detection failure or gathering information on operator decisions during development. + +![brief](./docs/traffic_light/traffic_light.svg) + +### Limitations + +This module allows developers to design STOP/GO in traffic light module using specific rules. Due to the property of rule-based planning, the algorithm is greatly depends on object detection and perception accuracy considering traffic light. Also, this module only handles STOP/Go at traffic light scene, so rushing or quick decision according to traffic condition is future work. + +#### Launch Timing + +Launches when there is traffic light in ego lane. + +#### Algorithm + +1. Obtains a traffic light mapped to the route and a stop line correspond to the traffic light from a map information. + +2. Uses the highest reliability one of the traffic light recognition result and if the color of that was red, generates a stop point. + +3. When vehicle current velocity is + + - higher than 2.0m/s ⇒ pass judge(using next slide formula) + + - lower than 2.0m/s ⇒ stop + +4. When it to be judged that vehicle can’t stop before stop line, autoware chooses one of the following behaviors + + - "can pass through" stop line during yellow lamp => pass + + - "can’t pass through" stop line during yellow lamp => emergency stop + +#### Dilemma Zone + +![brief](./docs/traffic_light/traffic_light_dilemma.svg) + +- yellow lamp line + + It’s called “yellow lamp line” which shows the distance traveled by the vehicle during yellow lamp. + +- dilemma zone + + It’s called “dilemma zone” which satisfies following conditions: + + - vehicle can’t pass through stop line during yellow lamp.(right side of the yellow lamp line) + + - vehicle can’t stop under deceleration and jerk limit.(left side of the pass judge curve) + + ⇒emergency stop(relax deceleration and jerk limitation in order to observe the traffic regulation) + +- optional zone + + It’s called “optional zone” which satisfies following conditions: + + - vehicle can pass through stop line during yellow lamp.(left side of the yellow lamp line) + + - vehicle can stop under deceleration and jerk limit.(right side of the pass judge curve) + + ⇒ stop(autoware selects the safety choice) + +#### Module Parameters + +| Parameter | Type | Description | +| --------------------------- | ------ | ----------------------------------------------- | +| `stop_margin` | double | [m] margin before stop point | +| `tl_state_timeout` | double | [s] time out for detected traffic light result. | +| `external_tl_state_timeout` | double | [s] time out for external traffic input | +| `yellow_lamp_period` | double | [s] time for yellow lamp | +| `enable_pass_judge` | bool | [-] weather to use pass judge | + +#### Flowchart + +```plantuml +@startuml +title modifyPathVelocity +start + +:calc stop point and insert index; + +:find offset segment; + +:calculate stop pose; + +:calculate distance to stop line; + +if (state is APPROACH) then (yes) + :set stop velocity; + if (distance to stop point is below singed dead line length(-2[m])) then (yes) + :change state to GO_OUT; + stop + elseif (no stop signal) then (yes) + :change previous state to STOP; + stop + elseif (not pass through) then (yes) + :insert stop pose; + :change previous state to STOP; + stop + else(no) + stop + endif +elseif(state is GO_OUT) then (yes) + if (signed arc length to stop line is more than restart length(1[m])) then (yes) + :change state to APPROACH; + endif + stop +else(no) + stop +endif + +@enduml +``` + +##### Known Limits + +- tbd.