From 135d778557c9751f2eb7012f7fa2f531ef2a08b8 Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Fri, 27 Sep 2024 16:06:35 +0900 Subject: [PATCH 01/36] support new perception_reproducer Signed-off-by: temkei.kem <1041084556@qq.com> --- perception/multi_object_tracker/package.xml | 1 + .../perception_replayer.py | 0 .../perception_replayer_common.py | 185 ++++++++++++++++++ .../old_reproducer/utils.py | 152 ++++++++++++++ .../perception_replayer_common.py | 12 +- .../perception_reproducer.py | 10 +- .../scripts/perception_replayer/utils.py | 4 +- 7 files changed, 347 insertions(+), 17 deletions(-) rename planning/planning_debug_tools/scripts/perception_replayer/{ => old_reproducer}/perception_replayer.py (100%) create mode 100644 planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py create mode 100644 planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 119919b5f076c..779acd0583233 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -25,6 +25,7 @@ tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs + diagnostic_updater ament_lint_auto autoware_lint_common diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer.py similarity index 100% rename from planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py rename to planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer.py diff --git a/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py new file mode 100644 index 0000000000000..22a73fab5199a --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py @@ -0,0 +1,185 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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. + +import os +from subprocess import CalledProcessError +from subprocess import check_output +import time + +from autoware_auto_perception_msgs.msg import DetectedObjects +from autoware_auto_perception_msgs.msg import PredictedObjects +from autoware_auto_perception_msgs.msg import TrackedObjects +from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray +from autoware_perception_msgs.msg import TrafficSignalArray +from geometry_msgs.msg import PoseWithCovarianceStamped +from nav_msgs.msg import Odometry +import psutil +from rclpy.node import Node +from rclpy.serialization import deserialize_message +from rosbag2_py import StorageFilter +from rosidl_runtime_py.utilities import get_message +from sensor_msgs.msg import PointCloud2 +from utils import open_reader + + +class PerceptionReplayerCommon(Node): + def __init__(self, args, name): + super().__init__(name) + self.args = args + + self.ego_pose = None + self.rosbag_objects_data = [] + self.rosbag_ego_odom_data = [] + self.rosbag_traffic_signals_data = [] + self.is_auto_traffic_signals = False + + # subscriber + self.sub_odom = self.create_subscription( + Odometry, "/localization/kinematic_state", self.on_odom, 1 + ) + + # publisher + if self.args.detected_object: + self.objects_pub = self.create_publisher( + DetectedObjects, "/perception/object_recognition/detection/objects", 1 + ) + elif self.args.tracked_object: + self.objects_pub = self.create_publisher( + TrackedObjects, "/perception/object_recognition/tracking/objects", 1 + ) + else: + self.objects_pub = self.create_publisher( + PredictedObjects, "/perception/object_recognition/objects", 1 + ) + + self.pointcloud_pub = self.create_publisher( + PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 + ) + self.recorded_ego_pub_as_initialpose = self.create_publisher( + PoseWithCovarianceStamped, "/initialpose", 1 + ) + + self.recorded_ego_pub = self.create_publisher( + Odometry, "/perception_reproducer/rosbag_ego_odom", 1 + ) + + # load rosbag + print("Stared loading rosbag") + if os.path.isdir(args.bag): + for bag_file in sorted(os.listdir(args.bag)): + if bag_file.endswith(self.args.rosbag_format): + self.load_rosbag(args.bag + "/" + bag_file) + else: + self.load_rosbag(args.bag) + print("Ended loading rosbag") + + # temporary support old auto msgs + if self.is_auto_traffic_signals: + self.auto_traffic_signals_pub = self.create_publisher( + AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + else: + self.traffic_signals_pub = self.create_publisher( + TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 + ) + + # wait for ready to publish/subscribe + time.sleep(1.0) + + def on_odom(self, odom): + self.ego_pose = odom.pose.pose + + def load_rosbag(self, rosbag2_path: str): + reader = open_reader(str(rosbag2_path)) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + objects_topic = ( + "/perception/object_recognition/detection/objects" + if self.args.detected_object + else "/perception/object_recognition/tracking/objects" + if self.args.tracked_object + else "/perception/object_recognition/objects" + ) + ego_odom_topic = "/localization/kinematic_state" + traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" + topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic]) + reader.set_filter(topic_filter) + + while reader.has_next(): + (topic, data, stamp) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + # import pdb; pdb.set_trace() + print(type(msg)) + if topic == objects_topic: + self.rosbag_objects_data.append((stamp, msg)) + if topic == ego_odom_topic: + self.rosbag_ego_odom_data.append((stamp, msg)) + if topic == traffic_signals_topic: + self.rosbag_traffic_signals_data.append((stamp, msg)) + self.is_auto_traffic_signals = ( + "autoware_auto_perception_msgs" in type(msg).__module__ + ) + + def kill_online_perception_node(self): + # kill node if required + kill_process_name = None + if self.args.detected_object: + kill_process_name = "dummy_perception_publisher_node" + elif self.args.tracked_object: + kill_process_name = "multi_object_tracker" + else: + kill_process_name = "map_based_prediction" + if kill_process_name: + try: + pid = check_output(["pidof", kill_process_name]) + process = psutil.Process(int(pid[:-1])) + process.terminate() + except CalledProcessError: + pass + + def binary_search(self, data, timestamp): + if data[-1][0] < timestamp: + return data[-1][1] + elif data[0][0] > timestamp: + return data[0][1] + + low, high = 0, len(data) - 1 + + while low <= high: + mid = (low + high) // 2 + if data[mid][0] < timestamp: + low = mid + 1 + elif data[mid][0] > timestamp: + high = mid - 1 + else: + return data[mid][1] + + # Return the next timestamp's data if available + if low < len(data): + return data[low][1] + return None + + def find_topics_by_timestamp(self, timestamp): + objects_data = self.binary_search(self.rosbag_objects_data, timestamp) + traffic_signals_data = self.binary_search(self.rosbag_traffic_signals_data, timestamp) + return objects_data, traffic_signals_data + + def find_ego_odom_by_timestamp(self, timestamp): + return self.binary_search(self.rosbag_ego_odom_data, timestamp) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py new file mode 100644 index 0000000000000..7e8e0a18b4b7c --- /dev/null +++ b/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 + +# Copyright 2023 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. + +import math +import time + +from geometry_msgs.msg import Quaternion +import numpy as np +import rosbag2_py +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from tf_transformations import euler_from_quaternion +from tf_transformations import quaternion_from_euler + + +def get_rosbag_options(path, serialization_format="cdr"): + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format, + ) + + return storage_options, converter_options + + +def open_reader(path: str): + storage_options, converter_options = get_rosbag_options(path) + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + return reader + + +def calc_squared_distance(p1, p2): + return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2) + + +def create_empty_pointcloud(timestamp): + pointcloud_msg = PointCloud2() + pointcloud_msg.header.stamp = timestamp + pointcloud_msg.header.frame_id = "map" + pointcloud_msg.height = 1 + pointcloud_msg.is_dense = True + pointcloud_msg.point_step = 16 + field_name_vec = ["x", "y", "z"] + offset_vec = [0, 4, 8] + for field_name, offset in zip(field_name_vec, offset_vec): + field = PointField() + field.name = field_name + field.offset = offset + field.datatype = 7 + field.count = 1 + pointcloud_msg.fields.append(field) + return pointcloud_msg + + +def get_yaw_from_quaternion(orientation): + orientation_list = [ + orientation.x, + orientation.y, + orientation.z, + orientation.w, + ] + return euler_from_quaternion(orientation_list)[2] + + +def get_quaternion_from_yaw(yaw): + q = quaternion_from_euler(0, 0, yaw) + orientation = Quaternion() + orientation.x = q[0] + orientation.y = q[1] + orientation.z = q[2] + orientation.w = q[3] + return orientation + + +def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg): + log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) + log_ego_pose_trans_mat = np.array( + [ + [ + math.cos(log_ego_yaw), + -math.sin(log_ego_yaw), + log_ego_pose.position.x, + ], + [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + ego_yaw = get_yaw_from_quaternion(ego_pose.orientation) + ego_pose_trans_mat = np.array( + [ + [math.cos(ego_yaw), -math.sin(ego_yaw), ego_pose.position.x], + [math.sin(ego_yaw), math.cos(ego_yaw), ego_pose.position.y], + [0.0, 0.0, 1.0], + ] + ) + + for o in objects_msg.objects: + log_object_pose = o.kinematics.pose_with_covariance.pose + log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) + log_object_pos_vec = np.array([log_object_pose.position.x, log_object_pose.position.y, 1.0]) + + # translate object pose from ego pose in log to ego pose in simulation + object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( + log_ego_pose_trans_mat.dot(log_object_pos_vec.T) + ) + + object_pose = o.kinematics.pose_with_covariance.pose + object_pose.position.x = object_pos_vec[0] + object_pose.position.y = object_pos_vec[1] + object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw) + + +class StopWatch: + def __init__(self, verbose): + # A dictionary to store the starting times + self.start_times = {} + self.verbose = verbose + + def tic(self, name): + """Store the current time with the given name.""" + self.start_times[name] = time.perf_counter() + + def toc(self, name): + """Print the elapsed time since the last call to tic() with the same name.""" + if name not in self.start_times: + print(f"No start time found for {name}!") + return + + elapsed_time = ( + time.perf_counter() - self.start_times[name] + ) * 1000 # Convert to milliseconds + if self.verbose: + print(f"Time for {name}: {elapsed_time: .2f} ms") + + # Reset the starting time for the name + del self.start_times[name] diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 2d876e2c93a0a..dbdf1f14e5a20 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -128,17 +128,13 @@ def load_rosbag(self, rosbag2_path: str): msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) if topic == objects_topic: - assert isinstance( - msg, self.objects_pub.msg_type - ), f"Unsupported conversion from {type(msg)}" + assert isinstance(msg, self.objects_pub.msg_type),f"Unsupported conversion from {type(msg)}" self.rosbag_objects_data.append((stamp, msg)) if topic == ego_odom_topic: - assert isinstance(msg, Odometry), f"Unsupported conversion from {type(msg)}" + assert isinstance(msg, Odometry),f"Unsupported conversion from {type(msg)}" self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: - assert isinstance( - msg, TrafficSignalArray - ), f"Unsupported conversion from {type(msg)}" + assert isinstance(msg, TrafficSignalArray),f"Unsupported conversion from {type(msg)}" self.rosbag_traffic_signals_data.append((stamp, msg)) def kill_online_perception_node(self): @@ -189,4 +185,4 @@ def find_topics_by_timestamp(self, timestamp): return objects_data, traffic_signals_data def find_ego_odom_by_timestamp(self, timestamp): - return self.binary_search(self.rosbag_ego_odom_data, timestamp) + return self.binary_search(self.rosbag_ego_odom_data, timestamp) \ No newline at end of file diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 3f63321b0fba2..c796ed684309f 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -176,12 +176,8 @@ def on_timer(self): (ego_pose.position.x - ego_odom_msg.pose.pose.position.x) ** 2 + (ego_pose.position.y - ego_odom_msg.pose.pose.position.y) ** 2 ) - repeat_flag = ( - ego_rosbag_speed > ego_speed * 2 - and ego_rosbag_speed > 3.0 - and ego_rosbag_dist > self.ego_odom_search_radius - ) - # if ego_rosbag_speed is too fast than ego_speed, stop publishing the rosbag's ego odom message temporarily. + repeat_flag = ego_rosbag_speed > ego_speed * 5 and ego_rosbag_dist > 1.0 + # set the speed threshold to many (5) times then ego_speed because this constraint is mainly for departing/stopping (ego speed is close to 0). if not repeat_flag: self.stopwatch.tic("find_topics_by_timestamp") @@ -328,4 +324,4 @@ def add_perception_noise( pass finally: node.destroy_node() - rclpy.shutdown() + rclpy.shutdown() \ No newline at end of file diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py index 22b4296bdef94..1b121f6c8efd0 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -151,7 +151,7 @@ def toc(self, name): time.perf_counter() - self.start_times[name] ) * 1000 # Convert to milliseconds if self.verbose: - print(f"Time for {name}: {elapsed_time: .2f} ms") + print(f"Time for {name}: {elapsed_time:.2f} ms") # Reset the starting time for the name - del self.start_times[name] + del self.start_times[name] \ No newline at end of file From 96c3d26bbcc491b4d9b52e29db65870ce7c3c625 Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Fri, 27 Sep 2024 18:55:49 +0900 Subject: [PATCH 02/36] move files Signed-off-by: temkei.kem <1041084556@qq.com> --- .../{old_reproducer => }/perception_replayer.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename planning/planning_debug_tools/scripts/perception_replayer/{old_reproducer => }/perception_replayer.py (100%) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py similarity index 100% rename from planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer.py rename to planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py From ec44ce693439cadabed9a9e88c3cbb45bc97ca26 Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Mon, 30 Sep 2024 12:04:05 +0900 Subject: [PATCH 03/36] remove old files. Signed-off-by: temkei.kem <1041084556@qq.com> --- .../perception_replayer_common.py | 185 ------------------ .../old_reproducer/utils.py | 152 -------------- 2 files changed, 337 deletions(-) delete mode 100644 planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py delete mode 100644 planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py diff --git a/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py deleted file mode 100644 index 22a73fab5199a..0000000000000 --- a/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/perception_replayer_common.py +++ /dev/null @@ -1,185 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 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. - -import os -from subprocess import CalledProcessError -from subprocess import check_output -import time - -from autoware_auto_perception_msgs.msg import DetectedObjects -from autoware_auto_perception_msgs.msg import PredictedObjects -from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray -from autoware_perception_msgs.msg import TrafficSignalArray -from geometry_msgs.msg import PoseWithCovarianceStamped -from nav_msgs.msg import Odometry -import psutil -from rclpy.node import Node -from rclpy.serialization import deserialize_message -from rosbag2_py import StorageFilter -from rosidl_runtime_py.utilities import get_message -from sensor_msgs.msg import PointCloud2 -from utils import open_reader - - -class PerceptionReplayerCommon(Node): - def __init__(self, args, name): - super().__init__(name) - self.args = args - - self.ego_pose = None - self.rosbag_objects_data = [] - self.rosbag_ego_odom_data = [] - self.rosbag_traffic_signals_data = [] - self.is_auto_traffic_signals = False - - # subscriber - self.sub_odom = self.create_subscription( - Odometry, "/localization/kinematic_state", self.on_odom, 1 - ) - - # publisher - if self.args.detected_object: - self.objects_pub = self.create_publisher( - DetectedObjects, "/perception/object_recognition/detection/objects", 1 - ) - elif self.args.tracked_object: - self.objects_pub = self.create_publisher( - TrackedObjects, "/perception/object_recognition/tracking/objects", 1 - ) - else: - self.objects_pub = self.create_publisher( - PredictedObjects, "/perception/object_recognition/objects", 1 - ) - - self.pointcloud_pub = self.create_publisher( - PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 - ) - self.recorded_ego_pub_as_initialpose = self.create_publisher( - PoseWithCovarianceStamped, "/initialpose", 1 - ) - - self.recorded_ego_pub = self.create_publisher( - Odometry, "/perception_reproducer/rosbag_ego_odom", 1 - ) - - # load rosbag - print("Stared loading rosbag") - if os.path.isdir(args.bag): - for bag_file in sorted(os.listdir(args.bag)): - if bag_file.endswith(self.args.rosbag_format): - self.load_rosbag(args.bag + "/" + bag_file) - else: - self.load_rosbag(args.bag) - print("Ended loading rosbag") - - # temporary support old auto msgs - if self.is_auto_traffic_signals: - self.auto_traffic_signals_pub = self.create_publisher( - AutoTrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) - else: - self.traffic_signals_pub = self.create_publisher( - TrafficSignalArray, "/perception/traffic_light_recognition/traffic_signals", 1 - ) - - # wait for ready to publish/subscribe - time.sleep(1.0) - - def on_odom(self, odom): - self.ego_pose = odom.pose.pose - - def load_rosbag(self, rosbag2_path: str): - reader = open_reader(str(rosbag2_path)) - - topic_types = reader.get_all_topics_and_types() - # Create a map for quicker lookup - type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} - - objects_topic = ( - "/perception/object_recognition/detection/objects" - if self.args.detected_object - else "/perception/object_recognition/tracking/objects" - if self.args.tracked_object - else "/perception/object_recognition/objects" - ) - ego_odom_topic = "/localization/kinematic_state" - traffic_signals_topic = "/perception/traffic_light_recognition/traffic_signals" - topic_filter = StorageFilter(topics=[objects_topic, ego_odom_topic, traffic_signals_topic]) - reader.set_filter(topic_filter) - - while reader.has_next(): - (topic, data, stamp) = reader.read_next() - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - # import pdb; pdb.set_trace() - print(type(msg)) - if topic == objects_topic: - self.rosbag_objects_data.append((stamp, msg)) - if topic == ego_odom_topic: - self.rosbag_ego_odom_data.append((stamp, msg)) - if topic == traffic_signals_topic: - self.rosbag_traffic_signals_data.append((stamp, msg)) - self.is_auto_traffic_signals = ( - "autoware_auto_perception_msgs" in type(msg).__module__ - ) - - def kill_online_perception_node(self): - # kill node if required - kill_process_name = None - if self.args.detected_object: - kill_process_name = "dummy_perception_publisher_node" - elif self.args.tracked_object: - kill_process_name = "multi_object_tracker" - else: - kill_process_name = "map_based_prediction" - if kill_process_name: - try: - pid = check_output(["pidof", kill_process_name]) - process = psutil.Process(int(pid[:-1])) - process.terminate() - except CalledProcessError: - pass - - def binary_search(self, data, timestamp): - if data[-1][0] < timestamp: - return data[-1][1] - elif data[0][0] > timestamp: - return data[0][1] - - low, high = 0, len(data) - 1 - - while low <= high: - mid = (low + high) // 2 - if data[mid][0] < timestamp: - low = mid + 1 - elif data[mid][0] > timestamp: - high = mid - 1 - else: - return data[mid][1] - - # Return the next timestamp's data if available - if low < len(data): - return data[low][1] - return None - - def find_topics_by_timestamp(self, timestamp): - objects_data = self.binary_search(self.rosbag_objects_data, timestamp) - traffic_signals_data = self.binary_search(self.rosbag_traffic_signals_data, timestamp) - return objects_data, traffic_signals_data - - def find_ego_odom_by_timestamp(self, timestamp): - return self.binary_search(self.rosbag_ego_odom_data, timestamp) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py deleted file mode 100644 index 7e8e0a18b4b7c..0000000000000 --- a/planning/planning_debug_tools/scripts/perception_replayer/old_reproducer/utils.py +++ /dev/null @@ -1,152 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 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. - -import math -import time - -from geometry_msgs.msg import Quaternion -import numpy as np -import rosbag2_py -from sensor_msgs.msg import PointCloud2 -from sensor_msgs.msg import PointField -from tf_transformations import euler_from_quaternion -from tf_transformations import quaternion_from_euler - - -def get_rosbag_options(path, serialization_format="cdr"): - storage_options = rosbag2_py.StorageOptions(uri=path, storage_id="sqlite3") - - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, - output_serialization_format=serialization_format, - ) - - return storage_options, converter_options - - -def open_reader(path: str): - storage_options, converter_options = get_rosbag_options(path) - reader = rosbag2_py.SequentialReader() - reader.open(storage_options, converter_options) - return reader - - -def calc_squared_distance(p1, p2): - return math.sqrt((p1.x - p2.x) ** 2 + (p1.y - p2.y) ** 2) - - -def create_empty_pointcloud(timestamp): - pointcloud_msg = PointCloud2() - pointcloud_msg.header.stamp = timestamp - pointcloud_msg.header.frame_id = "map" - pointcloud_msg.height = 1 - pointcloud_msg.is_dense = True - pointcloud_msg.point_step = 16 - field_name_vec = ["x", "y", "z"] - offset_vec = [0, 4, 8] - for field_name, offset in zip(field_name_vec, offset_vec): - field = PointField() - field.name = field_name - field.offset = offset - field.datatype = 7 - field.count = 1 - pointcloud_msg.fields.append(field) - return pointcloud_msg - - -def get_yaw_from_quaternion(orientation): - orientation_list = [ - orientation.x, - orientation.y, - orientation.z, - orientation.w, - ] - return euler_from_quaternion(orientation_list)[2] - - -def get_quaternion_from_yaw(yaw): - q = quaternion_from_euler(0, 0, yaw) - orientation = Quaternion() - orientation.x = q[0] - orientation.y = q[1] - orientation.z = q[2] - orientation.w = q[3] - return orientation - - -def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg): - log_ego_yaw = get_yaw_from_quaternion(log_ego_pose.orientation) - log_ego_pose_trans_mat = np.array( - [ - [ - math.cos(log_ego_yaw), - -math.sin(log_ego_yaw), - log_ego_pose.position.x, - ], - [math.sin(log_ego_yaw), math.cos(log_ego_yaw), log_ego_pose.position.y], - [0.0, 0.0, 1.0], - ] - ) - - ego_yaw = get_yaw_from_quaternion(ego_pose.orientation) - ego_pose_trans_mat = np.array( - [ - [math.cos(ego_yaw), -math.sin(ego_yaw), ego_pose.position.x], - [math.sin(ego_yaw), math.cos(ego_yaw), ego_pose.position.y], - [0.0, 0.0, 1.0], - ] - ) - - for o in objects_msg.objects: - log_object_pose = o.kinematics.pose_with_covariance.pose - log_object_yaw = get_yaw_from_quaternion(log_object_pose.orientation) - log_object_pos_vec = np.array([log_object_pose.position.x, log_object_pose.position.y, 1.0]) - - # translate object pose from ego pose in log to ego pose in simulation - object_pos_vec = np.linalg.inv(ego_pose_trans_mat).dot( - log_ego_pose_trans_mat.dot(log_object_pos_vec.T) - ) - - object_pose = o.kinematics.pose_with_covariance.pose - object_pose.position.x = object_pos_vec[0] - object_pose.position.y = object_pos_vec[1] - object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw) - - -class StopWatch: - def __init__(self, verbose): - # A dictionary to store the starting times - self.start_times = {} - self.verbose = verbose - - def tic(self, name): - """Store the current time with the given name.""" - self.start_times[name] = time.perf_counter() - - def toc(self, name): - """Print the elapsed time since the last call to tic() with the same name.""" - if name not in self.start_times: - print(f"No start time found for {name}!") - return - - elapsed_time = ( - time.perf_counter() - self.start_times[name] - ) * 1000 # Convert to milliseconds - if self.verbose: - print(f"Time for {name}: {elapsed_time: .2f} ms") - - # Reset the starting time for the name - del self.start_times[name] From 070f2388d16b4a4eab11a2ddb6222cfb48e246bc Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Mon, 30 Sep 2024 17:21:04 +0900 Subject: [PATCH 04/36] fix pre-commit err Signed-off-by: temkei.kem <1041084556@qq.com> --- .../perception_replayer/perception_replayer_common.py | 6 +++--- .../scripts/perception_replayer/utils.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index dbdf1f14e5a20..8095724d7c681 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -128,13 +128,13 @@ def load_rosbag(self, rosbag2_path: str): msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) if topic == objects_topic: - assert isinstance(msg, self.objects_pub.msg_type),f"Unsupported conversion from {type(msg)}" + assert isinstance(msg, self.objects_pub.msg_type), f"Unsupported conversion from {type(msg)}" self.rosbag_objects_data.append((stamp, msg)) if topic == ego_odom_topic: - assert isinstance(msg, Odometry),f"Unsupported conversion from {type(msg)}" + assert isinstance(msg, Odometry), f"Unsupported conversion from {type(msg)}" self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: - assert isinstance(msg, TrafficSignalArray),f"Unsupported conversion from {type(msg)}" + assert isinstance(msg, TrafficSignalArray), f"Unsupported conversion from {type(msg)}" self.rosbag_traffic_signals_data.append((stamp, msg)) def kill_online_perception_node(self): diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py index 1b121f6c8efd0..c799b2c2c60bb 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -151,7 +151,7 @@ def toc(self, name): time.perf_counter() - self.start_times[name] ) * 1000 # Convert to milliseconds if self.verbose: - print(f"Time for {name}: {elapsed_time:.2f} ms") + print(f"Time for {name}: {elapsed_time: .2f} ms") # Reset the starting time for the name del self.start_times[name] \ No newline at end of file From 0c6ad99514a6b3943ae3a431959829e5da10279d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 30 Sep 2024 08:35:05 +0000 Subject: [PATCH 05/36] style(pre-commit): autofix --- perception/multi_object_tracker/package.xml | 1 - .../perception_replayer/perception_replayer_common.py | 10 +++++++--- .../perception_replayer/perception_reproducer.py | 2 +- .../scripts/perception_replayer/utils.py | 2 +- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 779acd0583233..119919b5f076c 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -25,7 +25,6 @@ tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs - diagnostic_updater ament_lint_auto autoware_lint_common diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 8095724d7c681..2d876e2c93a0a 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -128,13 +128,17 @@ def load_rosbag(self, rosbag2_path: str): msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) if topic == objects_topic: - assert isinstance(msg, self.objects_pub.msg_type), f"Unsupported conversion from {type(msg)}" + assert isinstance( + msg, self.objects_pub.msg_type + ), f"Unsupported conversion from {type(msg)}" self.rosbag_objects_data.append((stamp, msg)) if topic == ego_odom_topic: assert isinstance(msg, Odometry), f"Unsupported conversion from {type(msg)}" self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: - assert isinstance(msg, TrafficSignalArray), f"Unsupported conversion from {type(msg)}" + assert isinstance( + msg, TrafficSignalArray + ), f"Unsupported conversion from {type(msg)}" self.rosbag_traffic_signals_data.append((stamp, msg)) def kill_online_perception_node(self): @@ -185,4 +189,4 @@ def find_topics_by_timestamp(self, timestamp): return objects_data, traffic_signals_data def find_ego_odom_by_timestamp(self, timestamp): - return self.binary_search(self.rosbag_ego_odom_data, timestamp) \ No newline at end of file + return self.binary_search(self.rosbag_ego_odom_data, timestamp) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index c796ed684309f..b096aa3445f0b 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -324,4 +324,4 @@ def add_perception_noise( pass finally: node.destroy_node() - rclpy.shutdown() \ No newline at end of file + rclpy.shutdown() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py index c799b2c2c60bb..22b4296bdef94 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -154,4 +154,4 @@ def toc(self, name): print(f"Time for {name}: {elapsed_time: .2f} ms") # Reset the starting time for the name - del self.start_times[name] \ No newline at end of file + del self.start_times[name] From a3e7da6e4376e4c8619fd0c7a487338fb77e3315 Mon Sep 17 00:00:00 2001 From: "temkei.kem" <1041084556@qq.com> Date: Fri, 4 Oct 2024 18:23:34 +0900 Subject: [PATCH 06/36] fix a small bug about perception reproducer --- .../scripts/perception_replayer/perception_reproducer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index b096aa3445f0b..022b73ee805b2 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -176,8 +176,8 @@ def on_timer(self): (ego_pose.position.x - ego_odom_msg.pose.pose.position.x) ** 2 + (ego_pose.position.y - ego_odom_msg.pose.pose.position.y) ** 2 ) - repeat_flag = ego_rosbag_speed > ego_speed * 5 and ego_rosbag_dist > 1.0 - # set the speed threshold to many (5) times then ego_speed because this constraint is mainly for departing/stopping (ego speed is close to 0). + repeat_flag = ego_rosbag_speed > ego_speed * 2 and ego_rosbag_speed > 3.0 and ego_rosbag_dist > self.ego_odom_search_radius + # if ego_rosbag_speed is too fast than ego_speed, stop publishing the rosbag's ego odom message temporarily. if not repeat_flag: self.stopwatch.tic("find_topics_by_timestamp") From 654b9edaf44760c064fd75be36dd6946dce60bfc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 4 Oct 2024 09:48:18 +0000 Subject: [PATCH 07/36] style(pre-commit): autofix --- .../scripts/perception_replayer/perception_reproducer.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 022b73ee805b2..3f63321b0fba2 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -176,7 +176,11 @@ def on_timer(self): (ego_pose.position.x - ego_odom_msg.pose.pose.position.x) ** 2 + (ego_pose.position.y - ego_odom_msg.pose.pose.position.y) ** 2 ) - repeat_flag = ego_rosbag_speed > ego_speed * 2 and ego_rosbag_speed > 3.0 and ego_rosbag_dist > self.ego_odom_search_radius + repeat_flag = ( + ego_rosbag_speed > ego_speed * 2 + and ego_rosbag_speed > 3.0 + and ego_rosbag_dist > self.ego_odom_search_radius + ) # if ego_rosbag_speed is too fast than ego_speed, stop publishing the rosbag's ego odom message temporarily. if not repeat_flag: From 3043778b7380a033cb404228adfc5218aaa6272d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 21 Oct 2024 18:09:12 +0900 Subject: [PATCH 08/36] fix(avoidance): don't insert stop line if the ego can't avoid or return (#9089) (#1592) * fix(avoidance): don't insert stop line if the ego can't avoid or return (#9089) * fix(avoidance): don't insert stop line if the ego can't avoid or return Signed-off-by: satoshi-ota * fix: build error Signed-off-by: satoshi-ota * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp Co-authored-by: Go Sakayori --------- Signed-off-by: satoshi-ota Co-authored-by: Go Sakayori * fix: build error Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota Co-authored-by: Go Sakayori --- .../behavior_path_avoidance_module/helper.hpp | 13 +++++++++++++ .../src/scene.cpp | 19 +++++++++++++++++++ 2 files changed, 32 insertions(+) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index 4ff02df97bd89..f656176193f59 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -312,6 +312,19 @@ class AvoidanceHelper }); } + bool isFeasible(const AvoidLineArray & shift_lines) const + { + constexpr double JERK_BUFFER = 0.1; // [m/sss] + const auto & values = parameters_->velocity_map; + const auto idx = getConstraintsMapIndex(0.0, values); // use minimum avoidance speed + const auto jerk_limit = parameters_->lateral_max_jerk_map.at(idx); + return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { + return PathShifter::calcJerkFromLatLonDistance( + line.getRelativeLength(), line.getRelativeLongitudinal(), values.at(idx)) < + jerk_limit + JERK_BUFFER; + }); + } + bool isReady(const ObjectDataArray & objects) const { if (objects.empty()) { diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index b7a732359e601..4af3dfdb68009 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1505,6 +1505,11 @@ void AvoidanceModule::insertReturnDeadLine( { const auto & data = avoid_data_; + if (data.new_shift_line.empty()) { + RCLCPP_WARN(getLogger(), "module doesn't have return shift line."); + return; + } + if (data.to_return_point > planner_data_->parameters.forward_path_length) { RCLCPP_DEBUG(getLogger(), "return dead line is far enough."); return; @@ -1517,6 +1522,11 @@ void AvoidanceModule::insertReturnDeadLine( return; } + if (!helper_->isFeasible(data.new_shift_line)) { + RCLCPP_WARN(getLogger(), "return shift line is not feasible. do nothing.."); + return; + } + // Consider the difference in path length between the shifted path and original path (the path // that is shifted inward has a shorter distance to the end of the path than the other one.) const auto & to_reference_path_end = data.arclength_from_ego.back(); @@ -1527,6 +1537,10 @@ void AvoidanceModule::insertReturnDeadLine( const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; + if (to_stop_line < 0.0) { + RCLCPP_WARN(getLogger(), "ego overran return shift dead line. do nothing."); + return; + } // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately @@ -1594,6 +1608,11 @@ void AvoidanceModule::insertWaitPoint( return; } + if (data.to_stop_line < 0.0) { + RCLCPP_WARN(getLogger(), "ego overran avoidance dead line. do nothing."); + return; + } + // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately if (!use_constraints_for_decel) { From da36aadb424ff294c72c10df26d68bf1bac6bce5 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 Jan 2024 08:14:33 +0900 Subject: [PATCH 09/36] feat(behavior_path_planner): output manager internal state as topic (#6099) Signed-off-by: satoshi-ota --- .../include/behavior_path_planner/planner_manager.hpp | 3 +++ .../behavior_path_planner/src/planner_manager.cpp | 11 +++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 3ccc92d04025f..5d145aa3e690b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -46,6 +46,7 @@ using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = tier4_autoware_utils::DebugPublisher; using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; +using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; enum Action { ADD = 0, @@ -447,6 +448,8 @@ class PlannerManager std::unique_ptr debug_publisher_ptr_; + std::unique_ptr state_publisher_ptr_; + pluginlib::ClassLoader plugin_loader_; mutable rclcpp::Logger logger_; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index fa5a745be9f0e..474cc34a85325 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -40,6 +40,7 @@ PlannerManager::PlannerManager( { processing_time_.emplace("total_time", 0.0); debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); + state_publisher_ptr_ = std::make_unique(&node, "~/debug"); } void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name) @@ -889,10 +890,6 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) void PlannerManager::print() const { - if (!verbose_) { - return; - } - const auto get_status = [](const auto & m) { return magic_enum::enum_name(m->getCurrentStatus()); }; @@ -941,6 +938,12 @@ void PlannerManager::print() const << std::setw(21); } + state_publisher_ptr_->publish("internal_state", string_stream.str()); + + if (!verbose_) { + return; + } + RCLCPP_INFO_STREAM(logger_, string_stream.str()); } From 1624ee11b10badf9bf6afe8807be9899d21232ad Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 18 Jan 2024 05:55:13 +0900 Subject: [PATCH 10/36] feat(rviz_plugin): add string visualization plugin (#6100) Signed-off-by: satoshi-ota --- common/tier4_debug_rviz_plugin/CMakeLists.txt | 2 + .../classes/StringStampedOverlayDisplay.png | Bin 0 -> 18815 bytes .../string_stamped.hpp | 107 ++++++++++ .../plugins/plugin_description.xml | 5 + .../src/string_stamped.cpp | 198 ++++++++++++++++++ 5 files changed, 312 insertions(+) create mode 100644 common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png create mode 100644 common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp create mode 100644 common/tier4_debug_rviz_plugin/src/string_stamped.cpp diff --git a/common/tier4_debug_rviz_plugin/CMakeLists.txt b/common/tier4_debug_rviz_plugin/CMakeLists.txt index 05cf35b93ef9f..02e65ad759ed6 100644 --- a/common/tier4_debug_rviz_plugin/CMakeLists.txt +++ b/common/tier4_debug_rviz_plugin/CMakeLists.txt @@ -12,7 +12,9 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(tier4_debug_rviz_plugin SHARED include/tier4_debug_rviz_plugin/float32_multi_array_stamped_pie_chart.hpp include/tier4_debug_rviz_plugin/jsk_overlay_utils.hpp + include/tier4_debug_rviz_plugin/string_stamped.hpp src/float32_multi_array_stamped_pie_chart.cpp + src/string_stamped.cpp src/jsk_overlay_utils.cpp ) diff --git a/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png b/common/tier4_debug_rviz_plugin/icons/classes/StringStampedOverlayDisplay.png new file mode 100644 index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663 GIT binary patch literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq literal 0 HcmV?d00001 diff --git a/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp new file mode 100644 index 0000000000000..0960875d7eee2 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/include/tier4_debug_rviz_plugin/string_stamped.hpp @@ -0,0 +1,107 @@ +// Copyright 2023 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ +#define TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace rviz_plugins +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage(const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + tier4_debug_msgs::msg::StringStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // TIER4_DEBUG_RVIZ_PLUGIN__STRING_STAMPED_HPP_ diff --git a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml index c1158465e1cf1..e18900afc8d84 100644 --- a/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_debug_rviz_plugin/plugins/plugin_description.xml @@ -4,4 +4,9 @@ base_class_type="rviz_common::Display"> Display drivable area of tier4_debug_msgs::msg::Float32MultiArrayStamped + + Display drivable area of tier4_debug_msgs::msg::StringStamped + diff --git a/common/tier4_debug_rviz_plugin/src/string_stamped.cpp b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp new file mode 100644 index 0000000000000..538dc0cbe7069 --- /dev/null +++ b/common/tier4_debug_rviz_plugin/src/string_stamped.cpp @@ -0,0 +1,198 @@ +// Copyright 2023 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "tier4_debug_rviz_plugin/string_stamped.hpp" + +#include "tier4_debug_rviz_plugin/jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + std::lock_guard message_lock(mutex_); + if (!last_msg_ptr_) { + return; + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + last_msg_ptr_->data.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const tier4_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::StringStampedOverlayDisplay, rviz_common::Display) From 7db969f0eaf830e1b5dfacb6a69a045b5f3ca3a7 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 22 Mar 2024 19:14:33 +0900 Subject: [PATCH 11/36] fix(motion_utils): check size after overlap points removal (#6653) * fix(motion_utils): check size after overlap points removal Signed-off-by: Muhammad Zulfaqar Azmi * change implementation to not return warning Signed-off-by: Muhammad Zulfaqar Azmi * fix comparison sign Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../include/motion_utils/trajectory/trajectory.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f2e0c1c0c184c..6cb7d34e3aab1 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -590,8 +590,12 @@ double calcLateralOffset( return std::nan(""); } - const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx)); - const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1)); + const auto p_indices = overlap_removed_points.size() - 2; + const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices; + const auto p_back_idx = p_front_idx + 1; + + const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx)); + const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx)); const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; From 7576be49b95a5f03f1711b87a98815993e403617 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 28 Oct 2024 19:35:06 +0900 Subject: [PATCH 12/36] fix(static_obstacle_avoidance): suppress unnecessary warning (#9142) (#1606) Signed-off-by: satoshi-ota --- planning/behavior_path_avoidance_module/src/scene.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 4af3dfdb68009..d8d1ee11e7d8f 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -1505,11 +1505,6 @@ void AvoidanceModule::insertReturnDeadLine( { const auto & data = avoid_data_; - if (data.new_shift_line.empty()) { - RCLCPP_WARN(getLogger(), "module doesn't have return shift line."); - return; - } - if (data.to_return_point > planner_data_->parameters.forward_path_length) { RCLCPP_DEBUG(getLogger(), "return dead line is far enough."); return; @@ -1522,6 +1517,11 @@ void AvoidanceModule::insertReturnDeadLine( return; } + if (data.new_shift_line.empty()) { + RCLCPP_WARN(getLogger(), "module doesn't have return shift line."); + return; + } + if (!helper_->isFeasible(data.new_shift_line)) { RCLCPP_WARN(getLogger(), "return shift line is not feasible. do nothing.."); return; From 1530ea306e626070b01cc7c0057fd57c6b326d4d Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 20 Dec 2023 13:01:05 +0900 Subject: [PATCH 13/36] feat(intersection): disable stuck detection against private lane (#5910) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 3 +-- .../src/manager.cpp | 9 +++------ .../src/scene_intersection.cpp | 10 ++++++---- .../src/scene_intersection.hpp | 10 +++------- 4 files changed, 13 insertions(+), 19 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 997addd48d7f8..6fc136c512689 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -23,8 +23,7 @@ stuck_vehicle_velocity_threshold: 0.833 # enable_front_car_decel_prediction: false # assumed_front_car_decel: 1.0 - timeout_private_area: 3.0 - enable_private_area_stuck_disregard: false + disable_against_private_lane: true yield_stuck: turn_direction: diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 789708abe98f8..e859b15b345b8 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -74,10 +74,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); ip.stuck_vehicle.stuck_vehicle_velocity_threshold = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); - ip.stuck_vehicle.timeout_private_area = - getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); - ip.stuck_vehicle.enable_private_area_stuck_disregard = - getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); ip.yield_stuck.turn_direction.left = getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); @@ -201,7 +199,6 @@ void IntersectionModuleManager::launchNewModules( } const std::string location = ll.attributeOr("location", "else"); - const bool is_private_area = (location.compare("private") == 0); const auto associative_ids = planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); bool has_traffic_light = false; @@ -213,7 +210,7 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, enable_occlusion_detection, is_private_area, node_, + has_traffic_light, enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index e036fac13fd3b..5909d607f6351 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -97,8 +97,8 @@ IntersectionModule::IntersectionModule( [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), @@ -107,7 +107,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), - is_private_area_(is_private_area), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -1056,8 +1055,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + const bool is_first_conflicting_lane_private = + (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { - if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { + if (!(is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane)) { } else { std::optional stopline_idx = std::nullopt; if (stuck_stopline_idx_opt) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7366bdc1bc0e6..2f8c296a348b9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -74,8 +74,7 @@ class IntersectionModule : public SceneModuleInterface bool use_stuck_stopline; double stuck_vehicle_detect_dist; double stuck_vehicle_velocity_threshold; - double timeout_private_area; - bool enable_private_area_stuck_disregard; + bool disable_against_private_lane; } stuck_vehicle; struct YieldStuck @@ -263,8 +262,8 @@ class IntersectionModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, + const rclcpp::Clock::SharedPtr clock); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path @@ -314,9 +313,6 @@ class IntersectionModule : public SceneModuleInterface // vehicles are very slow std::optional initial_green_light_observed_time_{std::nullopt}; - // for stuck vehicle detection - const bool is_private_area_; - // for RTC const UUID occlusion_uuid_; bool occlusion_safety_{true}; From 3836123dc5ca2731476b399e4d0fcaf2c58bca59 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 27 Dec 2023 19:46:50 +0900 Subject: [PATCH 14/36] fix(intersection): fix private condition (#5975) --- .../src/scene_intersection.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 5909d607f6351..631c03d923da0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1058,8 +1058,10 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_first_conflicting_lane_private = (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { - if (!(is_first_conflicting_lane_private && - planner_param_.stuck_vehicle.disable_against_private_lane)) { + if ( + is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane) { + // do nothing } else { std::optional stopline_idx = std::nullopt; if (stuck_stopline_idx_opt) { From b5ad8f8ed7395efa94be6ad72d06ad96c5a6b4c1 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 4 Jan 2024 17:13:04 +0900 Subject: [PATCH 15/36] refactor(intersection): cleanup utility function (#5972) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 1422 +++++++++++++++-- .../src/scene_intersection.hpp | 490 +++++- .../src/scene_merge_from_private_road.cpp | 77 +- .../src/scene_merge_from_private_road.hpp | 8 +- .../src/util.cpp | 1389 +--------------- .../src/util.hpp | 176 +- .../src/util_type.hpp | 172 +- 7 files changed, 1885 insertions(+), 1849 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 631c03d923da0..292a6e6a8843d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,11 @@ #include "util.hpp" #include +#include #include +#include +#include +#include #include #include #include @@ -37,10 +41,27 @@ #include #include +#include #include #include #include #include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -71,6 +92,25 @@ Polygon2d createOneStepPolygon( } } // namespace +static bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) +{ + 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::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -376,7 +416,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -392,7 +432,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, - [[maybe_unused]] util::DebugData * debug_data) + [[maybe_unused]] DebugData * debug_data) { return; } @@ -403,7 +443,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -451,7 +491,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -483,7 +523,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -526,7 +566,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -577,7 +617,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -634,7 +674,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -681,7 +721,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -734,7 +774,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -776,7 +816,7 @@ void reactRTCApprovalByDecisionResult( const IntersectionModule::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -818,7 +858,7 @@ void reactRTCApproval( const IntersectionModule::DecisionResult & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) + VelocityFactorInterface * velocity_factor, DebugData * debug_data) { std::visit( VisitorSwitch{[&](const auto & decision) { @@ -868,7 +908,7 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { - debug_data_ = util::DebugData(); + debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); // set default RTC @@ -938,6 +978,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const auto & current_pose = planner_data_->current_odometry->pose; // spline interpolation const auto interpolated_path_info_opt = util::generateInterpolatedPath( @@ -952,25 +994,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } // cache intersection lane information because it is invariant - const auto & current_pose = planner_data_->current_odometry->pose; const auto lanelets_on_path = planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); if (!intersection_lanelets_) { - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.common.attention_area_length, - planner_param_.occlusion.occlusion_attention_area_length, - planner_param_.collision_detection.consider_wrong_direction_vehicle); + intersection_lanelets_ = + getObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path); } auto & intersection_lanelets = intersection_lanelets_.value(); - // at the very first time of registration of this module, the path may not be conflicting with the + // at the very first time of regisTration of this module, the path may not be conflicting with the // attention area, so update() is called to update the internal data as well as traffic light info const auto traffic_prioritized_level = - util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); // this is abnormal @@ -985,21 +1022,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // generate all stop line candidates // see the doc for struct IntersectionStopLines - const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); /// even if the attention area is null, stuck vehicle stop line needs to be generated from /// conflicting lanes - const auto & dummy_first_attention_area = - first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; - const auto & dummy_first_attention_lane_centerline = - intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value().centerline2d() - : first_conflicting_lane.centerline2d(); - const auto intersection_stoplines_opt = util::generateIntersectionStopLines( - first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline, - planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.default_stopline_margin, planner_param_.common.max_accel, - planner_param_.common.max_jerk, planner_param_.common.delay_response_time, - planner_param_.occlusion.peeking_offset, path); + const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value() + : first_conflicting_lane; + + const auto intersection_stoplines_opt = generateIntersectionStopLines( + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, interpolated_path_info, + path); if (!intersection_stoplines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } @@ -1010,15 +1041,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; // see the doc for struct PathLanelets + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, - conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + const auto path_lanelets_opt = generatePathLanelets( + lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, + first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } - const auto path_lanelets = path_lanelets_opt.value(); + const auto & path_lanelets = path_lanelets_opt.value(); // utility functions auto fromEgoDist = [&](const size_t index) { @@ -1054,7 +1085,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation - const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets, &debug_data_); const bool is_first_conflicting_lane_private = (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); if (stuck_detected) { @@ -1119,11 +1150,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // check occlusion on detection lane if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( + occlusion_attention_divisions_ = generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution, - planner_param_.occlusion.attention_lane_crop_curvature_threshold, - planner_param_.occlusion.attention_lane_curvature_calculation_ds); + planner_data_->occupancy_grid->info.resolution); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); @@ -1132,8 +1161,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const bool yield_stuck_detected = checkYieldStuckVehicle( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding_); + const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(), + &debug_data_); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -1159,15 +1189,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const double is_amber_or_red = - (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); + (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? getOcclusionStatus( - *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area, interpolated_path_info, occlusion_attention_divisions, - target_objects, current_pose, - planner_param_.occlusion.occlusion_required_clearance_distance) + occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, + occlusion_attention_divisions, target_objects) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -1385,9 +1413,672 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } -bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) +TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return TrafficPrioritizedLevel::NOT_PRIORITIZED; + } + const auto & tl_info = tl_info_it->second; + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; + } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; + } + } + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + } + return TrafficPrioritizedLevel::NOT_PRIORITIZED; +} + +static std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +IntersectionLanelets IntersectionModule::getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path) +{ + const double detection_area_length = planner_param_.common.attention_area_length; + const double occlusion_detection_area_length = + planner_param_.occlusion.occlusion_attention_area_length; + const bool consider_wrong_direction_vehicle = + planner_param_.collision_detection.consider_wrong_direction_vehicle; + + // retrieve a stopline associated with a traffic light + bool has_traffic_light = false; + if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + + // for low priority lane + // If ego_lane has right of way (i.e. is high priority), + // ignore yieldLanelets (i.e. low priority lanes) + lanelet::ConstLanelets yield_lanelets{}; + 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); + } + } + } + } + + // get all following lanes of previous lane + lanelet::ConstLanelets ego_lanelets = lanelets_on_path; + 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 adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } + + // final objective lanelets + lanelet::ConstLanelets detection_lanelets; + lanelet::ConstLanelets conflicting_ex_ego_lanelets; + // conflicting lanes is necessary to get stopline for stuck vehicle + for (auto && conflicting_lanelet : conflicting_lanelets) { + if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) + conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); + } + + // exclude yield lanelets and ego lanelets from detection_lanelets + if (turn_direction_ == std::string("straight") && has_traffic_light) { + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + } else { + if (consider_wrong_direction_vehicle) { + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); + } + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if ( + lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || + lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + } + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + lanelet::ConstLanelets detection_and_preceding_lanelets; + { + const double length = detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.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, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } + } + } + } + + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.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, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + + IntersectionLanelets result; + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stopline{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + if (stopline) break; + } + result.attention_stoplines_.push_back(stopline); + } + result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + } + result.attention_non_preceding_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); + result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched + result.attention_area_ = getPolygon3dFromLanelets(result.attention_); + result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); + result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); + result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); + result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); + return result; +} + +std::optional IntersectionModule::generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) +{ + const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; + const double stopline_margin = planner_param_.common.default_stopline_margin; + const double max_accel = planner_param_.common.max_accel; + const double max_jerk = planner_param_.common.max_jerk; + const double delay_response_time = planner_param_.common.delay_response_time; + const double peeking_offset = planner_param_.occlusion.peeking_offset; + + const auto first_attention_area = first_attention_lane.polygon3d(); + const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); + const auto & path_ip = interpolated_path_info.path; + const double ds = interpolated_path_info.ds; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); + const int base2front_idx_dist = + std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); + + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // NOTE: maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + + // (1) default stop line position on interpolated path + bool default_stopline_valid = true; + int stop_idx_ip_int = -1; + if (const auto map_stop_idx_ip = + getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); + map_stop_idx_ip) { + stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stopline_valid = false; + } + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + + // (2) ego front stop line position on interpolated path + const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + + // (3) occlusion peeking stop line position on interpolated path + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); + bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the detection area, invalid + { + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = + first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); + } + + const auto occlusion_peeking_line_ip = static_cast( + std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; + const bool first_attention_stopline_valid = true; + + // (4) pass judge line position on interpolated path + const double velocity = planner_data_->current_velocity->twist.linear.x; + const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; + const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); + const auto pass_judge_line_ip = static_cast( + std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + // TODO(Mamoru Sobue): maybe braking dist should be considered + const auto occlusion_wo_tl_pass_judge_line_ip = + static_cast(first_footprint_attention_centerline_ip); + + // (5) stuck vehicle stop line + int stuck_stopline_ip_int = 0; + bool stuck_stopline_valid = true; + if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching detection area and already passed + // first_conflicting_area, this could be null. + const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); + if (!stuck_stopline_idx_ip_opt) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; + } else { + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; + } + } else { + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; + } + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; + size_t pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( + [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { + const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; + const auto insert_idx = util::insertPointIndex( + insert_point, original_path, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + if (!insert_idx) { + return std::nullopt; + } + *stop_idx = insert_idx.value(); + } + if ( + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; + } + + IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; + } + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; + } + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; + } + if (occlusion_peeking_line_valid) { + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; + } + intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; + intersection_stoplines.occlusion_wo_tl_pass_judge_line = + intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; + return intersection_stoplines; +} + +/** + * @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. + */ +std::optional IntersectionModule::getStopLineIndexFromMap( + const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) +{ + const auto & path = interpolated_path_info.path; + const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); + + const auto road_markings = + assigned_lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stopline; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. + } + } + if (stopline.empty()) { + return std::nullopt; + } + + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = + planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); + + for (size_t i = lane_interval.first; i < lane_interval.second; 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_stopline, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + return i; + } + + geometry_msgs::msg::Pose stop_point_from_map; + stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); + + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); +} + +static lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets previous_lanelets; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return previous_lanelets; + } + previous_lanelets.push_back(ll); + } + return previous_lanelets; +} + +// end inclusive +static lanelet::ConstLanelet generatePathLanelet( + const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, + const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + size_t prev_idx = start_idx; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + prev_idx = i; + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + // NOTE: maybe this is opposite + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + +static std::optional> +getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons, const bool search_forward = true) +{ + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const 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) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const 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) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } + } + } + return std::nullopt; +} + +std::optional IntersectionModule::generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx) { + const double width = planner_data_->vehicle_info_.vehicle_width_m; + static constexpr double path_lanelet_interval = 1.5; + + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; + + PathLanelets path_lanelets; + // prev + path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids_); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } + return path_lanelets; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + const PathLanelets & path_lanelets, DebugData * debug_data) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + const bool stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || @@ -1397,22 +2088,146 @@ bool IntersectionModule::checkStuckVehicle( return false; } - const auto & objects_ptr = planner_data->predicted_objects; + const auto & objects_ptr = planner_data_->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path - const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); + const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; + Polygon2d stuck_vehicle_detect_area{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return false; + } + + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } + const auto target_polygon = + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); + + if (target_polygon.empty()) { + return false; + } + + for (const auto & p : target_polygon) { + stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); + } + + stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); + bg::correct(stuck_vehicle_detect_area); + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - return util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, &debug_data_); + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area && debug_data) { + debug_data->stuck_targets.objects.push_back(object); + return true; + } + } + return false; } -bool IntersectionModule::checkYieldStuckVehicle( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets) +static lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +static lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +bool IntersectionModule::checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) { const bool yield_stuck_detection_direction = [&]() { return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || @@ -1423,20 +2238,74 @@ bool IntersectionModule::checkYieldStuckVehicle( return false; } - return util::checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, attention_lanelets, turn_direction_, - planner_data_->vehicle_info_.vehicle_width_m, - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, - planner_param_.yield_stuck.distance_threshold, &debug_data_); + const double width = planner_data_->vehicle_info_.vehicle_width_m; + const double stuck_vehicle_vel_thr = + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; + const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; + + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction_ == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction_ == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { + continue; + } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data->yield_stuck_targets.objects.push_back(object.object); + return true; + } + } + } + return false; } -util::TargetObjects IntersectionModule::generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, +TargetObjects IntersectionModule::generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - util::TargetObjects target_objects; + TargetObjects target_objects; target_objects.header = objects_ptr->header; const auto & attention_lanelets = intersection_lanelets.attention(); const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); @@ -1449,10 +2318,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects( // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, false); + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); if (belong_adjacent_lanelet_id) { continue; } @@ -1466,33 +2333,28 @@ util::TargetObjects IntersectionModule::generateTargetObjects( const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); if (belong_attention_lanelet_id) { const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); container.push_back(target_object); } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = std::nullopt; target_object.stopline = std::nullopt; target_objects.intersection_area_objects.push_back(target_object); } - } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, - planner_param_.common.attention_area_angle_threshold, - planner_param_.collision_detection.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, is_parked_vehicle); + } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( + object_direction, attention_lanelets, is_parked_vehicle); belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before const auto id = belong_attention_lanelet_id.value(); - util::TargetObject target_object; + TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stopline = attention_lanelet_stoplines.at(id); @@ -1512,10 +2374,10 @@ util::TargetObjects IntersectionModule::generateTargetObjects( } bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1523,14 +2385,8 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, lane_id_, associative_ids_, closest_idx, - last_intersection_stopline_candidate_idx, time_delay, - planner_param_.collision_detection.velocity_profile.default_velocity, - planner_param_.collision_detection.velocity_profile.minimum_default_velocity, - planner_param_.collision_detection.velocity_profile.use_upstream, - planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity, - &ego_ttc_time_array); + const auto time_distance_array = calcIntersectionPassingTime( + path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); if ( std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != @@ -1540,7 +2396,7 @@ bool IntersectionModule::checkCollision( } const double passing_time = time_distance_array.back().first; - util::cutPredictPathWithDuration(target_objects, clock_, passing_time); + cutPredictPathWithDuration(target_objects, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( @@ -1551,12 +2407,12 @@ bool IntersectionModule::checkCollision( // change TTC margin based on ego traffic light color const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); } - if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { return std::make_pair( planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); @@ -1565,7 +2421,7 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_start_margin_time, planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); - const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { if (!target_object.dist_to_stopline) { return false; } @@ -1580,7 +2436,7 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stopline > braking_distance; }; - const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + const auto isTolerableOvershoot = [&](const TargetObject & target_object) { if ( !target_object.attention_lanelet || !target_object.dist_to_stopline || !target_object.stopline) { @@ -1639,14 +2495,14 @@ bool IntersectionModule::checkCollision( // If the vehicle is expected to stop before their stopline, ignore const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); if ( - traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && !expected_to_stop_before_stopline && is_tolerable_overshoot) { debug_data_.red_overshoot_ignore_targets.objects.push_back(object); continue; @@ -1766,16 +2622,288 @@ bool IntersectionModule::checkCollision( return collision_detected; } +std::optional IntersectionModule::checkAngleForTargetLanelets( + + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const +{ + const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; + const bool consider_wrong_direction_vehicle = + planner_param_.common.attention_area_angle_threshold; + const double dist_margin = planner_param_.common.attention_area_margin; + + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { + continue; + } + const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); + const double pose_angle = tf2::getYaw(pose.orientation); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); + } + } + } + return std::nullopt; +} + +void IntersectionModule::cutPredictPathWithDuration( + TargetObjects * target_objects, const double time_thr) +{ + const rclcpp::Time current_time = clock_->now(); + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.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(target_objects->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); + } + } + } + } +} + +TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) +{ + const double intersection_velocity = + planner_param_.collision_detection.velocity_profile.default_velocity; + const double minimum_ego_velocity = + planner_param_.collision_detection.velocity_profile.minimum_default_velocity; + const bool use_upstream_velocity = + planner_param_.collision_detection.velocity_profile.use_upstream; + const double minimum_upstream_velocity = + planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + + int assigned_lane_found = false; + + // crop intersection part of the path, and set the reference velocity to intersection_velocity + // for ego's ttc + PathWithLaneId reference_path; + std::optional upstream_stopline{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stopline_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stopline) { + upstream_stopline = i; + } + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } + reference_path.points.push_back(reference_point); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); + 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. + } + + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + + // apply smoother to reference velocity + PathWithLaneId smoothed_reference_path = reference_path; + if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { + smoothed_reference_path = reference_path; + } + + // calculate when ego is going to reach each (interpolated) points on the path + TimeDistanceArray time_distance_array{}; + double dist_sum = 0.0; + double passing_time = time_delay; + time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stopline_candidate_idx` makes no sense + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stopline_point, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); + const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); + + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (has_upstream_stopline && i > upstream_stopline_ind) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); + passing_time += (dist / passing_velocity); + + time_distance_array.emplace_back(passing_time, dist_sum); + } + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id_); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); + } + return time_distance_array; +} + +static double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + +std::vector IntersectionModule::generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) +{ + const double curvature_threshold = + planner_param_.occlusion.attention_lane_crop_curvature_threshold; + const double curvature_calculation_ds = + planner_param_.occlusion.attention_lane_curvature_calculation_ds; + + using lanelet::utils::getCenterlineWithOffset; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + const auto [merged_detection_lanelets, originals] = + util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); + double area = 0; + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); + } + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); + const double width = area / length; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); + } + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); + } + return detection_divisions; +} + IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr) + const TargetObjects & target_objects) { + const auto & occ_grid = *planner_data_->occupancy_grid; + const auto & current_pose = planner_data_->current_odometry->pose; + const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; + const auto & path_ip = interpolated_path_info.path; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); @@ -2094,6 +3222,76 @@ IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( return OcclusionType::STATICALLY_OCCLUDED; } +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + +void IntersectionLanelets::update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + is_prioritized_ = is_prioritized; + // find the first conflicting/detection area polygon intersecting the path + if (!first_conflicting_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); + } + } + if (!first_attention_area_) { + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); + } + } +} + +void TargetObject::calc_dist_to_stopline() +{ + if (!attention_lanelet || !stopline) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stopline_val = stopline.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; + stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; + stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); +} + /* bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 2f8c296a348b9..6a350e66706e6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -42,6 +43,259 @@ namespace behavior_velocity_planner using TimeDistanceArray = std::vector>; +struct DebugData +{ + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; + std::vector candidate_collision_object_polygons; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionLanelets + */ +struct IntersectionLanelets +{ +public: + /** + * update conflicting lanelets and traffic priority information + */ + void update( + const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + + const lanelet::ConstLanelets & attention() const + { + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stoplines() const + { + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; + } + const lanelet::ConstLanelets & conflicting() const { return conflicting_; } + const lanelet::ConstLanelets & adjacent() const { return adjacent_; } + const lanelet::ConstLanelets & occlusion_attention() const + { + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; + } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } + const std::vector & attention_area() const + { + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; + } + const std::vector & conflicting_area() const + { + return conflicting_area_; + } + const std::vector & adjacent_area() const { return adjacent_area_; } + const std::vector & occlusion_attention_area() const + { + return occlusion_attention_area_; + } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } + const std::optional & first_conflicting_area() const + { + return first_conflicting_area_; + } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } + + /** + * the set of attention lanelets which is topologically merged + */ + lanelet::ConstLanelets attention_; + std::vector attention_area_; + + /** + * the stop lines for each attention_ lanelets + */ + std::vector> attention_stoplines_; + + /** + * the conflicting part of attention lanelets + */ + lanelet::ConstLanelets attention_non_preceding_; + std::vector attention_non_preceding_area_; + + /** + * the stop lines for each attention_non_preceding_ + */ + std::vector> attention_non_preceding_stoplines_; + + /** + * the conflicting lanelets of the objective intersection lanelet + */ + lanelet::ConstLanelets conflicting_; + std::vector conflicting_area_; + + /** + * + */ + lanelet::ConstLanelets adjacent_; + std::vector adjacent_area_; + + /** + * the set of attention lanelets for occlusion detection which is topologically merged + */ + lanelet::ConstLanelets occlusion_attention_; + std::vector occlusion_attention_area_; + + /** + * the vector of sum of each occlusion_attention lanelet + */ + std::vector occlusion_attention_size_; + + /** + * the attention lanelet which ego path points intersect for the first time + */ + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + + /** + * the attention lanelet which ego path points intersect for the first time + */ + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + + /** + * flag if the intersection is prioritized by the traffic light + */ + bool is_prioritized_{false}; +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct IntersectionStopLines +{ + size_t closest_idx{0}; + + /** + * stuck_stopline is null if ego path does not intersect with first_conflicting_area + */ + std::optional stuck_stopline{std::nullopt}; + + /** + * default_stopline is null if it is calculated negative from first_attention_stopline + */ + std::optional default_stopline{std::nullopt}; + + /** + * first_attention_stopline is null if ego footprint along the path does not intersect with + * attention area. if path[0] satisfies the condition, it is 0 + */ + std::optional first_attention_stopline{std::nullopt}; + + /** + * occlusion_peeking_stopline is null if path[0] is already inside the attention area + */ + std::optional occlusion_peeking_stopline{std::nullopt}; + + /** + * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is + * calculated negative, it is 0 + */ + size_t pass_judge_line{0}; + + /** + * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with + * the centerline of the first_attention_lane + */ + size_t occlusion_wo_tl_pass_judge_line{0}; +}; + +/** + * @struct + * @brief see the document for more details of PathLanelets + */ +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the + // path +}; + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + std::optional dist_to_stopline{std::nullopt}; + void calc_dist_to_stopline(); +}; + +/** + * @struct + * @brief categorize TargetObjects + */ +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy +}; + +/** + * @struct + * @brief categorize traffic light priority + */ +enum class TrafficPrioritizedLevel { + //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + //! The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + //! The target lane's traffic signal is green + NOT_PRIORITIZED +}; + class IntersectionModule : public SceneModuleInterface { public: @@ -159,33 +413,57 @@ class IntersectionModule : public SceneModuleInterface }; enum OcclusionType { + //! occlusion is not detected NOT_OCCLUDED, + //! occlusion is not caused by dynamic objects STATICALLY_OCCLUDED, + //! occlusion is caused by dynamic objects DYNAMICALLY_OCCLUDED, - RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + //! actual occlusion does not exist, only disapproved by RTC + RTC_OCCLUDED, }; + /** + * @struct + * @brief Internal error or ego already passed pass_judge_line + */ struct Indecisive { std::string error; }; + /** + * @struct + * @brief detected stuck vehicle + */ struct StuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; std::optional occlusion_stopline_idx{std::nullopt}; }; + /** + * @struct + * @brief yielded by vehicle on the attention area + */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; }; + /** + * @struct + * @brief only collision is detected + */ struct NonOccludedCollisionStop { size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief occlusion is detected so ego needs to stop at the default stop line position + */ struct FirstWaitBeforeOcclusion { bool is_actually_occlusion_cleared{false}; @@ -193,23 +471,29 @@ class IntersectionModule : public SceneModuleInterface size_t first_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; - // A state peeking to occlusion limit line in the presence of traffic light + /** + * @struct + * @brief ego is approaching the boundary of attention area in the presence of traffic light + */ struct PeekingTowardOcclusion { - // NOTE: if intersection_occlusion is disapproved externally through RTC, - // it indicates "is_forcefully_occluded" + //! if intersection_occlusion is disapproved externally through RTC, it indicates + //! "is_forcefully_occluded" bool is_actually_occlusion_cleared{false}; bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck and shows up - // intersection_occlusion(x.y) + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck and shows up + //! intersection_occlusion(x.y) std::optional static_occlusion_timeout{std::nullopt}; }; - // A state detecting both collision and occlusion in the presence of traffic light + /** + * @struct + * @brief both collision and occlusion are detected in the presence of traffic light + */ struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; @@ -218,10 +502,14 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stopline_idx{0}; size_t first_attention_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) - // if valid, it contains the remaining time to release the static occlusion stuck + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; }; + /** + * @struct + * @brief at least occlusion is detected in the absence of traffic light + */ struct OccludedAbsenceTrafficLight { bool is_actually_occlusion_cleared{false}; @@ -231,13 +519,20 @@ class IntersectionModule : public SceneModuleInterface size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; }; + /** + * @struct + * @brief both collision and occlusion are not detected + */ struct Safe { - // NOTE: if RTC is disapproved status, default stop lines are still needed. size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; }; + /** + * @struct + * @brief traffic light is red or arrow signal + */ struct FullyPrioritized { bool collision_detected{false}; @@ -246,16 +541,16 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stopline_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected stuck vehicle - YieldStuckStop, // detected yield stuck vehicle - NonOccludedCollisionStop, // detected collision while FOV is clear - FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion - PeekingTowardOcclusion, // peeking into occlusion while collision is not detected - OccludedCollisionStop, // occlusion and collision are both detected - OccludedAbsenceTrafficLight, // occlusion is detected in the absence of traffic light - Safe, // judge as safe - FullyPrioritized // only detect vehicles violating traffic rules + Indecisive, //! internal process error, or over the pass judge line + StuckStop, //! detected stuck vehicle + YieldStuckStop, //! detected yield stuck vehicle + NonOccludedCollisionStop, //! detected collision while FOV is clear + FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion + PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected + OccludedCollisionStop, //! occlusion and collision are both detected + OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light + Safe, //! judge as safe + FullyPrioritized //! only detect vehicles violating traffic rules >; IntersectionModule( @@ -265,10 +560,6 @@ class IntersectionModule : public SceneModuleInterface const bool enable_occlusion_detection, rclcpp::Node & node, 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(PathWithLaneId * path, StopReason * stop_reason) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; @@ -289,28 +580,27 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; + // Parameter + PlannerParam planner_param_; + + std::optional intersection_lanelets_{std::nullopt}; + + // for pass judge decision bool is_go_out_{false}; bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; OcclusionType prev_occlusion_status_; - // Parameter - PlannerParam planner_param_; - - std::optional intersection_lanelets_{std::nullopt}; - // for occlusion detection const bool enable_occlusion_detection_; std::optional> occlusion_attention_divisions_{ - std::nullopt}; - StateMachine collision_state_machine_; //! for stable collision checking - StateMachine before_creep_state_machine_; //! for two phase stop - StateMachine occlusion_stop_state_machine_; + std::nullopt}; //! for caching discretized occlusion detection lanelets + StateMachine collision_state_machine_; //! for stable collision checking + StateMachine before_creep_state_machine_; //! for two phase stop + StateMachine occlusion_stop_state_machine_; //! for stable occlusion detection StateMachine temporal_stop_before_attention_state_machine_; StateMachine static_occlusion_timeout_state_machine_; - // for pseudo-collision detection when ego just entered intersection on green light and upcoming - // vehicles are very slow std::optional initial_green_light_observed_time_{std::nullopt}; // for RTC @@ -318,43 +608,135 @@ class IntersectionModule : public SceneModuleInterface bool occlusion_safety_{true}; double occlusion_stop_distance_{0.0}; bool occlusion_activated_{true}; - // for first stop in two-phase stop bool occlusion_first_stop_required_{false}; + /** + * @fn + * @brief set all RTC variable to safe and -inf + */ void initializeRTCStatus(); + /** + * @fn + * @brief analyze traffic_light/occupancy/objects context and return DecisionResult + */ + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief set RTC value according to calculated DecisionResult + */ void prepareRTCStatus( const DecisionResult &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + /** + * @fn + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel( + lanelet::ConstLanelet lane, const std::map & tl_infos); + + /** + * @fn + * @brief generate IntersectionLanelets + */ + IntersectionLanelets getObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path); - bool checkStuckVehicle( - const std::shared_ptr & planner_data, - const util::PathLanelets & path_lanelets); + /** + * @fn + * @brief generate IntersectionStopLines + */ + std::optional generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const util::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - bool checkYieldStuckVehicle( - const util::TargetObjects & target_objects, + /** + * @fn + * @brief find the associated stopline road marking of assigned lanelet + */ + std::optional getStopLineIndexFromMap( const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets); + lanelet::ConstLanelet assigned_lanelet); - util::TargetObjects generateTargetObjects( - const util::IntersectionLanelets & intersection_lanelets, + /** + * @fn + * @brief generate PathLanelets + */ + std::optional generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx); + + /** + * @fn + * @brief check stuck + */ + bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief check yield stuck + */ + bool checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data); + + /** + * @fn + * @brief categorize target objects + */ + TargetObjects generateTargetObjects( + const IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; + /** + * @fn + * @brief check collision + */ bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, - const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level); + + std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const; + + void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); + /** + * @fn + * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of + * (time of arrival, traveled distance) from current ego position + */ + TimeDistanceArray calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); + + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + + /** + * @fn + * @brief check occlusion status + */ OcclusionType getOcclusionStatus( - const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, - const double occlusion_dist_thr); + const TargetObjects & target_objects); /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -364,7 +746,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - util::DebugData debug_data_; + DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index b373f2cbc1c8a..f56d0a4adcff7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,17 +16,16 @@ #include "util.hpp" -#include #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -50,6 +49,32 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( state_machine_.setState(StateMachine::State::STOP); } +static std::optional getFirstConflictingLanelet( + const lanelet::ConstLanelets & conflicting_lanelets, + const util::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (const auto & conflicting_lanelet : conflicting_lanelets) { + const auto polygon_2d = conflicting_lanelet.polygon2d().basicPolygon(); + const bool intersects = bg::intersects(polygon_2d, path_footprint); + if (intersects) { + return std::make_optional(conflicting_lanelet); + } + } + } + return std::nullopt; +} + bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = DebugData(); @@ -83,41 +108,36 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR return false; } - /* get detection area */ - if (!intersection_lanelets_) { - const auto & assigned_lanelet = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_); - const auto lanelets_on_path = planning_utils::getLaneletsOnPath( - *path, lanelet_map_ptr, planner_data_->current_odometry->pose); - intersection_lanelets_ = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, - planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, - planner_param_.consider_wrong_direction_vehicle); - } - auto & intersection_lanelets = intersection_lanelets_.value(); const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - intersection_lanelets.update( - false, interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); - const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); - if (!first_conflicting_area) { + if (!first_conflicting_lanelet_) { + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, assigned_lanelet); + first_conflicting_lanelet_ = getFirstConflictingLanelet( + conflicting_lanelets, interpolated_path_info, local_footprint, + planner_data_->vehicle_info_.max_longitudinal_offset_m); + } + if (!first_conflicting_lanelet_) { return false; } + const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - /* set stop-line and stop-judgement-line for base_link */ - const auto stopline_idx_opt = util::generateStuckStopLine( - first_conflicting_area.value(), planner_data_, interpolated_path_info, - planner_param_.stopline_margin, false, path); - if (!stopline_idx_opt.has_value()) { - RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); + const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, + planner_data_->vehicle_info_.max_longitudinal_offset_m); + if (!first_conflicting_idx_opt) { return false; } + const auto stopline_idx_ip = first_conflicting_idx_opt.value(); - const size_t stopline_idx = stopline_idx_opt.value(); - if (stopline_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); + const auto stopline_idx_opt = util::insertPointIndex( + interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + if (!stopline_idx_opt) { + RCLCPP_DEBUG(logger_, "failed to insert stopline, ignore planning."); return true; } + const auto stopline_idx = stopline_idx_opt.value(); debug_data_.virtual_wall_pose = planning_utils::getAheadPose( stopline_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); @@ -195,4 +215,5 @@ MergeFromPrivateRoadModule::extractPathNearExitOfPrivateRoad( std::reverse(private_path.points.begin(), private_path.points.end()); return private_path; } + } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index fab0303640700..ab368e6b3106b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,10 +15,7 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include "scene_intersection.hpp" - #include -#include #include #include @@ -27,9 +24,6 @@ #include #include -#include -#include - #include #include #include @@ -89,7 +83,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional first_conflicting_lanelet_; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index e491d2ce7c5ce..6afc2d61d77f2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -18,15 +18,10 @@ #include #include -#include #include -#include -#include -#include #include #include #include -#include #include #include @@ -38,29 +33,12 @@ #include #include #include -#include #include #include #include -#include #include #include -namespace tier4_autoware_utils -{ - -template <> -inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - point.z = p.z(); - return point; -} - -} // namespace tier4_autoware_utils - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -84,7 +62,7 @@ static std::optional getDuplicatedPointIdx( return std::nullopt; } -static std::optional insertPointIndex( +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) @@ -156,66 +134,7 @@ std::optional> findLaneIdsInterval( return found ? std::make_optional(std::make_pair(start, end)) : std::nullopt; } -/** - * @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. - */ -static std::optional getStopLineIndexFromMap( - const InterpolatedPathInfo & interpolated_path_info, - const std::shared_ptr & planner_data) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - lanelet::ConstLanelet lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get( - interpolated_path_info.lane_id); - const auto road_markings = lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stopline; - for (const auto & road_marking : road_markings) { - const std::string type = - road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); - if (type == lanelet::AttributeValueString::StopLine) { - stopline.push_back(road_marking->roadMarking()); - break; // only one stopline exists. - } - } - if (stopline.empty()) { - return std::nullopt; - } - - const auto p_start = stopline.front().front(); - const auto p_end = stopline.front().back(); - const LineString2d extended_stopline = - planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length); - - for (size_t i = lane_interval.first; i < lane_interval.second; 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_stopline, path_segment, collision_points); - - if (collision_points.empty()) { - continue; - } - - return i; - } - - geometry_msgs::msg::Pose stop_point_from_map; - stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); - stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); - stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - - return motion_utils::findFirstNearestIndexWithSoftConstraints( - path.points, stop_point_from_map, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::optional getFirstPointInsidePolygonByFootprint( +std::optional getFirstPointInsidePolygonByFootprint( const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { @@ -236,222 +155,6 @@ static std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygonsByFootprint( - const std::vector & polygons, - const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (size_t j = 0; j < polygons.size(); ++j) { - const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); - const bool is_in_polygon = bg::intersects(area_2d, path_footprint); - if (is_in_polygon) { - return std::make_optional>(i, j); - } - } - } - return std::nullopt; -} - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const double baselink2front = planner_data->vehicle_info_.max_longitudinal_offset_m; - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - - // find the index of the first point whose vehicle footprint on it intersects with detection_area - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); - - std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; - for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // NOTE: maybe consideration of braking dist is necessary - first_footprint_attention_centerline_ip_opt = i; - break; - } - } - if (!first_footprint_attention_centerline_ip_opt) { - return std::nullopt; - } - const size_t first_footprint_attention_centerline_ip = - first_footprint_attention_centerline_ip_opt.value(); - - // (1) default stop line position on interpolated path - bool default_stopline_valid = true; - int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data); - map_stop_idx_ip) { - stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } - if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; - } - if (stop_idx_ip_int < 0) { - default_stopline_valid = false; - } - const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; - - // (2) ego front stop line position on interpolated path - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_ip.points, current_pose, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - - // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); - bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the detection area, invalid - { - const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { - occlusion_peeking_line_valid = false; - } - } - if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = - first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); - } - - const auto occlusion_peeking_line_ip = static_cast( - std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; - const bool first_attention_stopline_valid = true; - - // (4) pass judge line position on interpolated path - const double velocity = planner_data->current_velocity->twist.linear.x; - const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_accel, max_jerk, delay_response_time); - int pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line - int stuck_stopline_ip_int = 0; - bool stuck_stopline_valid = true; - if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection area and already passed - // first_conflicting_area, this could be null. - const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stopline_idx_ip_opt) { - stuck_stopline_valid = false; - stuck_stopline_ip_int = 0; - } else { - stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; - } - } else { - stuck_stopline_ip_int = - std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); - } - if (stuck_stopline_ip_int < 0) { - stuck_stopline_valid = false; - } - const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); - - struct IntersectionStopLinesTemp - { - size_t closest_idx{0}; - size_t stuck_stopline{0}; - size_t default_stopline{0}; - size_t first_attention_stopline{0}; - size_t occlusion_peeking_stopline{0}; - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; - }; - - IntersectionStopLinesTemp intersection_stoplines_temp; - std::list> stoplines = { - {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, - {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, - {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, - {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, - {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; - stoplines.sort( - [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stoplines) { - const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); - if (!insert_idx) { - return std::nullopt; - } - *stop_idx = insert_idx.value(); - } - if ( - intersection_stoplines_temp.occlusion_peeking_stopline < - intersection_stoplines_temp.default_stopline) { - intersection_stoplines_temp.occlusion_peeking_stopline = - intersection_stoplines_temp.default_stopline; - } - - IntersectionStopLines intersection_stoplines; - intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; - if (stuck_stopline_valid) { - intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; - } - if (default_stopline_valid) { - intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; - } - if (first_attention_stopline_valid) { - intersection_stoplines.first_attention_stopline = - intersection_stoplines_temp.first_attention_stopline; - } - if (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; - intersection_stoplines.occlusion_wo_tl_pass_judge_line = - intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stoplines; -} - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -490,105 +193,7 @@ std::optional getFirstPointInsidePolygon( return std::nullopt; } -static std::optional> -getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, - const std::vector & polygons, const bool search_forward = true) -{ - if (search_forward) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - const 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) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - } - } else { - for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { - bool is_in_lanelet = false; - const 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) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - if (i == 0) { - break; - } - } - } - return std::nullopt; -} - -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const auto lane_interval_ip_start = std::get<0>(lane_interval_ip); - size_t stuck_stopline_idx_ip = 0; - if (use_stuck_stopline) { - stuck_stopline_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stopline_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, conflicting_area); - if (!stuck_stopline_idx_ip_opt) { - return std::nullopt; - } - stuck_stopline_idx_ip = stuck_stopline_idx_ip_opt.value(); - } - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stopline_idx_ip) - 1 - stopline_margin_idx_dist - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - return insertPointIndex( - insert_point, original_path, planner_data->ego_nearest_dist_threshold, - planner_data->ego_nearest_yaw_threshold); -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - -/** - * @param pair lanelets and the vector of original lanelets in topological order (not reversed as - *in generateDetectionLaneDivisions()) - **/ -static std::pair> +std::pair> mergeLaneletsByTopologicalSort( const lanelet::ConstLanelets & lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr) @@ -683,207 +288,9 @@ mergeLaneletsByTopologicalSort( return {merged, originals}; } -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle) -{ - const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - - // for low priority lane - // If ego_lane has right of way (i.e. is high priority), - // ignore yieldLanelets (i.e. low priority lanes) - lanelet::ConstLanelets yield_lanelets{}; - 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); - } - } - } - } - - // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets = lanelets_on_path; - 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 adjacent_followings; - - for (const auto & conflicting_lanelet : conflicting_lanelets) { - for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - } - - // final objective lanelets - lanelet::ConstLanelets detection_lanelets; - lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stopline for stuck vehicle - for (auto && conflicting_lanelet : conflicting_lanelets) { - if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) - conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); - } - - // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction == std::string("straight") && has_traffic_light) { - // if assigned lanelet is "straight" with traffic light, detection area is not necessary - } else { - if (consider_wrong_direction_vehicle) { - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - for (const auto & adjacent_following : adjacent_followings) { - detection_lanelets.push_back(adjacent_following); - } - } else { - // otherwise we need to know the priority from RightOfWay - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if ( - lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || - lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - } - } - - // get possible lanelet path that reaches conflicting_lane longer than given length - lanelet::ConstLanelets detection_and_preceding_lanelets; - { - const double length = detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) detection_and_preceding_lanelets.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, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); - } - } - } - } - - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; - { - const double length = occlusion_detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.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, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); - } - } - } - } - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; - for (const auto & ll : occlusion_detection_and_preceding_lanelets) { - const auto turn_direction = getTurnDirection(ll); - if (turn_direction == "left" || turn_direction == "right") { - continue; - } - occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); - } - - auto [attention_lanelets, original_attention_lanelet_sequences] = - mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - - IntersectionLanelets result; - result.attention_ = std::move(attention_lanelets); - for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { - // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that - // back() exists. - std::optional stopline{std::nullopt}; - for (auto it = original_attention_lanelet_seq.rbegin(); - it != original_attention_lanelet_seq.rend(); ++it) { - const auto traffic_lights = it->regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - break; - } - if (stopline) break; - } - result.attention_stoplines_.push_back(stopline); - } - result.attention_non_preceding_ = std::move(detection_lanelets); - for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stopline = std::nullopt; - const auto & ll = result.attention_non_preceding_.at(i); - const auto traffic_lights = ll.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - } - result.attention_non_preceding_stoplines_.push_back(stopline); - } - result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - // NOTE: occlusion_attention is not inverted here - // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and - // then trim part of them based on curvature threshold - result.occlusion_attention_ = - std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); - - // NOTE: to properly update(), each element in conflicting_/conflicting_area_, - // attention_non_preceding_/attention_non_preceding_area_ need to be matched - result.attention_area_ = getPolygon3dFromLanelets(result.attention_); - result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); - result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); - result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); - result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); - return result; -} - bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -893,8 +300,8 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { const geometry_msgs::msg::Pose target_pose = path.points.at(target_idx).point.pose; @@ -903,81 +310,13 @@ bool isBeforeTargetIndex( return static_cast(target_idx > closest_idx); } -/* -static std::vector getAllAdjacentLanelets( - const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) -{ - std::set results; - - results.insert(lane.id()); - - auto it = routing_graph->adjacentRight(lane); - // take all lane on the right side - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentRight(it.get()); - } - // take all lane on the left side - it = routing_graph->adjacentLeft(lane); - while (!!it) { - results.insert(it.get().id()); - it = routing_graph->adjacentLeft(it.get()); - - } - return std::vector(results.begin(), results.end()); -} -*/ - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( - lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, - lanelet::ConstLanelet lane) -{ - // some of the intersections are not well-formed, and "adjacent" turning - // lanelets are not sharing the LineStrings - const std::string turn_direction = getTurnDirection(lane); - if (turn_direction != "left" && turn_direction != "right" && turn_direction != "straight") - return {}; - - std::set previous_lanelet_ids; - for (auto && previous_lanelet : routing_graph->previous(lane)) { - previous_lanelet_ids.insert(previous_lanelet.id()); - } - - std::set besides_previous_lanelet_ids; - for (auto && previous_lanelet_id : previous_lanelet_ids) { - lanelet::ConstLanelet previous_lanelet = map->laneletLayer.get(previous_lanelet_id); - for (auto && beside_lanelet : getAllAdjacentLanelets(routing_graph, previous_lanelet)) { - besides_previous_lanelet_ids.insert(beside_lanelet); - } - } - - std::set following_turning_lanelets; - following_turning_lanelets.insert(lane.id()); - for (auto && besides_previous_lanelet_id : besides_previous_lanelet_ids) { - lanelet::ConstLanelet besides_previous_lanelet = - map->laneletLayer.get(besides_previous_lanelet_id); - for (auto && following_lanelet : routing_graph->following(besides_previous_lanelet)) { - // if this has {"turn_direction", "${turn_direction}"}, take this - if (getTurnDirection(following_lanelet) == turn_direction) - following_turning_lanelets.insert(following_lanelet.id()); - } - } - lanelet::ConstLanelets ret{}; - for (auto && id : following_turning_lanelets) { - ret.push_back(map->laneletLayer.get(id)); - } - return ret; -} -*/ - -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr) { const std::string area_id_str = assigned_lane.attributeOr("intersection_area", "else"); if (area_id_str == "else") return std::nullopt; - const int area_id = std::atoi(area_id_str.c_str()); + const lanelet::Id area_id = std::atoi(area_id_str.c_str()); const auto poly_3d = lanelet_map_ptr->polygonLayer.get(area_id); Polygon2d poly{}; for (const auto & p : poly_3d) poly.outer().emplace_back(p.x(), p.y()); @@ -994,111 +333,6 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos) -{ - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { - // the info of this traffic light is not available - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_it->second; - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; - } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; -} - -double getHighestCurvature(const lanelet::ConstLineString3d & centerline) -{ - std::vector points; - for (auto point = centerline.begin(); point != centerline.end(); point++) { - points.push_back(*point); - } - - SplineInterpolationPoints2d interpolation(points); - const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); - std::vector curvatures_positive; - for (const auto & curvature : curvatures) { - curvatures_positive.push_back(std::fabs(curvature)); - } - return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); -} - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds) -{ - using lanelet::utils::getCenterlineWithOffset; - - // (0) remove left/right lanelet - lanelet::ConstLanelets detection_lanelets; - for (const auto & detection_lanelet : detection_lanelets_all) { - // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet - const auto fine_centerline = - lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); - const double highest_curvature = getHighestCurvature(fine_centerline); - if (highest_curvature > curvature_threshold) { - continue; - } - detection_lanelets.push_back(detection_lanelet); - } - - // (1) tsort detection_lanelets - const auto [merged_detection_lanelets, originals] = - mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); - - // (2) merge each branch to one lanelet - // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::vector> merged_lanelet_with_area; - for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { - const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); - const auto & original = originals.at(i); - double area = 0; - for (const auto & partition : original) { - area += bg::area(partition.polygon2d().basicPolygon()); - } - merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); - } - - // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { - const double length = bg::length(merged_lanelet.centerline()); - const double width = area / length; - for (int i = 0; i < static_cast(width / resolution); ++i) { - const double offset = resolution * i - width / 2; - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); - } - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); - } - return detection_divisions; -} - std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -1116,7 +350,6 @@ std::optional generateInterpolatedPath( return interpolated_path_info; } -// from here geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) { @@ -1134,609 +367,5 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - 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::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data) -{ - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; // not stop vehicle - } - - // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); - if (is_in_stuck_area && debug_data) { - debug_data->stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -static lanelet::LineString3d getLineStringFromArcLength( - const lanelet::ConstLineString3d & linestring, const double s1, const double s2) -{ - lanelet::Points3d points; - double accumulated_length = 0; - size_t start_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s1) { - start_index = i; - break; - } - accumulated_length += length; - } - if (start_index < linestring.size() - 1) { - const auto & p1 = linestring[start_index]; - const auto & p2 = linestring[start_index + 1]; - const double residue = s1 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto start_basic_point = p1.basicPoint() + residue * direction_vector; - const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); - points.push_back(start_point); - } - - accumulated_length = 0; - size_t end_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s2) { - end_index = i; - break; - } - accumulated_length += length; - } - - for (size_t i = start_index + 1; i < end_index; i++) { - const auto p = lanelet::Point3d(linestring[i]); - points.push_back(p); - } - if (end_index < linestring.size() - 1) { - const auto & p1 = linestring[end_index]; - const auto & p2 = linestring[end_index + 1]; - const double residue = s2 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto end_basic_point = p1.basicPoint() + residue * direction_vector; - const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); - points.push_back(end_point); - } - return lanelet::LineString3d{lanelet::InvalId, points}; -} - -static lanelet::ConstLanelet createLaneletFromArcLength( - const lanelet::ConstLanelet & lanelet, const double s1, const double s2) -{ - const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); - // make sure that s1, and s2 are between [0, lane_length] - const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); - const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); - - const auto ratio_s1 = s1_saturated / total_length; - const auto ratio_s2 = s2_saturated / total_length; - - const auto s1_left = - static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s2_left = - static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s1_right = - static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); - const auto s2_right = - static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); - - const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); - const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); - - return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); -} - -bool checkYieldStuckVehicleInIntersection( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, - const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data) -{ - LineString2d sparse_intersection_path; - const auto [start, end] = interpolated_path_info.lane_id_interval.value(); - for (unsigned i = start; i < end; ++i) { - const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; - const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); - if (turn_direction == "right") { - const double right_x = point.x - width / 2 * std::sin(yaw); - const double right_y = point.y + width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(right_x, right_y); - } else if (turn_direction == "left") { - const double left_x = point.x + width / 2 * std::sin(yaw); - const double left_y = point.y - width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(left_x, left_y); - } else { - // straight - sparse_intersection_path.emplace_back(point.x, point.y); - } - } - lanelet::ConstLanelets yield_stuck_detect_lanelets; - for (const auto & attention_lanelet : attention_lanelets) { - const auto centerline = attention_lanelet.centerline2d().basicLineString(); - std::vector intersects; - bg::intersection(sparse_intersection_path, centerline, intersects); - if (intersects.empty()) { - continue; - } - const auto intersect = intersects.front(); - const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( - centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); - const double yield_stuck_start = - std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); - const double yield_stuck_end = intersect_arc_coords.length; - yield_stuck_detect_lanelets.push_back( - createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); - } - debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { - const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); - - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; - } - for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { - const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); - if (is_in_lanelet) { - debug_data->yield_stuck_targets.objects.push_back(object.object); - return true; - } - } - } - return false; -} - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - Polygon2d polygon{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return polygon; - } - - double target_polygon_length = - getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); - lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; - if (path_lanelets.next) { - targets.push_back(path_lanelets.next.value()); - const double next_arc_length = - std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); - target_polygon_length += next_arc_length; - } - const auto target_polygon = - to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); - - 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()); - bg::correct(polygon); - - return polygon; -} - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle) -{ - for (unsigned i = 0; i < target_lanelets.size(); ++i) { - const auto & ll = target_lanelets.at(i); - if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { - continue; - } - const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); - const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); - if (consider_wrong_direction_vehicle) { - if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - } else { - if (std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is - // positive - if ( - is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return std::make_optional(i); - } - } - } - return std::nullopt; -} - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) -{ - const rclcpp::Time current_time = clock->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.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(target_objects->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); - } - } - } - } -} - -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - const double current_velocity = planner_data->current_velocity->twist.linear.x; - - int assigned_lane_found = false; - - // crop intersection part of the path, and set the reference velocity to intersection_velocity - // for ego's ttc - PathWithLaneId reference_path; - std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - auto reference_point = path.points.at(i); - // assume backward velocity is current ego velocity - if (i < closest_idx) { - reference_point.point.longitudinal_velocity_mps = current_velocity; - } - if ( - i > last_intersection_stopline_candidate_idx && - std::fabs(reference_point.point.longitudinal_velocity_mps) < - std::numeric_limits::epsilon() && - !upstream_stopline) { - upstream_stopline = i; - } - if (!use_upstream_velocity) { - reference_point.point.longitudinal_velocity_mps = intersection_velocity; - } - reference_path.points.push_back(reference_point); - bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids); - 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. - } - - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - - // apply smoother to reference velocity - PathWithLaneId smoothed_reference_path = reference_path; - if (!smoothPath(reference_path, smoothed_reference_path, planner_data)) { - smoothed_reference_path = reference_path; - } - - // calculate when ego is going to reach each (interpolated) points on the path - TimeDistanceArray time_distance_array{}; - double dist_sum = 0.0; - double passing_time = time_delay; - time_distance_array.emplace_back(passing_time, dist_sum); - - // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - - const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { - if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stopline_point, - planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - } else { - return std::nullopt; - } - }(); - const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); - - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { - const auto & p1 = smoothed_reference_path.points.at(i); - const auto & p2 = smoothed_reference_path.points.at(i + 1); - - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); - dist_sum += dist; - - // use average velocity between p1 and p2 - const double average_velocity = - (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double passing_velocity = [=]() { - if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { - return minimum_upstream_velocity; - } - return std::max(average_velocity, minimum_ego_velocity); - } else { - return std::max(average_velocity, minimum_ego_velocity); - } - }(); - passing_time += (dist / passing_velocity); - - time_distance_array.emplace_back(passing_time, dist_sum); - } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); - for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); - } - return time_distance_array; -} - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx) -{ - const auto lane_id = assigned_lanelet.id(); - const auto intersection_first_itr = - std::find_if(path.points.cbegin(), path.points.cend(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - 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 = std::abs(motion_utils::calcSignedArcLength(path.points, 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; -} - -void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - is_prioritized_ = is_prioritized; - // find the first conflicting/detection area polygon intersecting the path - if (!first_conflicting_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - conflicting_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_conflicting_lane_ = conflicting_.at(first.value().second); - first_conflicting_area_ = conflicting_area_.at(first.value().second); - } - } - if (!first_attention_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_attention_lane_ = attention_non_preceding_.at(first.value().second); - first_attention_area_ = attention_non_preceding_area_.at(first.value().second); - } - } -} - -static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) -{ - lanelet::ConstLanelets previous_lanelets; - for (const auto & ll : lanelets_on_path) { - if (associative_ids.find(ll.id()) != associative_ids.end()) { - return previous_lanelets; - } - previous_lanelets.push_back(ll); - } - return previous_lanelets; -} - -// end inclusive -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - size_t prev_idx = start_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } - prev_idx = i; - const double yaw = tf2::getYaw(p.orientation); - const double x = p.position.x; - const double y = p.position.y; - // NOTE: maybe this is opposite - const double left_x = x + width / 2 * std::sin(yaw); - const double left_y = y - width / 2 * std::cos(yaw); - const double right_x = x - width / 2 * std::sin(yaw); - const double right_y = y + width / 2 * std::cos(yaw); - lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); - rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width) -{ - static constexpr double path_lanelet_interval = 1.5; - - const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; - if (!assigned_lane_interval_opt) { - return std::nullopt; - } - const auto assigned_lane_interval = assigned_lane_interval_opt.value(); - const auto & path = interpolated_path_info.path; - - PathLanelets path_lanelets; - // prev - path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids); - path_lanelets.all = path_lanelets.prev; - - // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; - if (closest_idx > assigned_lane_start) { - path_lanelets.all.push_back( - generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); - } - - // ego_or_entry2exit - const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); - path_lanelets.ego_or_entry2exit = - generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); - - // next - if (assigned_lane_end < path.points.size() - 1) { - const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); - const auto next_lane_interval_opt = findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.next.value()); - } - } - - const auto first_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); - const auto last_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) - : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); - if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { - const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); - const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, - path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); - if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); - } - } - return path_lanelets; -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; - } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); -} - } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 33e511d911b82..86a34d1e95114 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,12 +15,12 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "scene_intersection.hpp" #include "util_type.hpp" #include -#include +#include +#include #include #include @@ -35,88 +35,82 @@ namespace behavior_velocity_planner { namespace util { -std::optional insertPoint( + +/** + * @fn + * @brief insert a new pose to the path and return its index + * @return if insertion was successful return the inserted point index + */ +std::optional insertPointIndex( const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); +/** + * @fn + * @brief check if a PathPointWithLaneId contains any of the given lane ids + */ bool hasLaneIds( const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); -std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief get objective polygons for detection area + * @fn + * @brief find the first contiguous interval of the path points that contains the specified lane ids + * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is + * found, returns the pair (start-1, end) */ -IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, - const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle); +std::optional> findLaneIdsInterval( + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @brief Generate a stop line for stuck vehicle - * @param conflicting_areas used to generate stop line for stuck vehicle - * @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 use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + * @fn + * @brief return the index of the first point which is inside the given polygon + * @param[in] lane_interval the interval of the path points on the intersection + * @param[in] search_forward flag for search direction */ -std::optional generateStuckStopLine( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - -std::optional generateIntersectionStopLines( - const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::CompoundPolygon3d & first_attention_area, - const lanelet::ConstLineString2d & first_attention_lane_centerline, - const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stopline_margin, const double max_accel, const double max_jerk, - const double delay_response_time, const double peeking_offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); - std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); /** + * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose - * @param path path - * @param closest_idx ego's closest index on the path - * @param current_pose ego's exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); +/** + * @fn + * @brief check if ego is before the target_idx. If the index is same, compare the exact pose + * @param[in] path path + * @param[in] closest_idx ego's closest index on the path + * @param[in] current_pose ego's exact pose + * @return true if ego is over the target_idx + */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const geometry_msgs::msg::Pose & current_pose, const int target_idx); - -/* -lanelet::ConstLanelets extendedAdjacentDirectionLanes( -lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, -lanelet::ConstLanelet lane); -*/ + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); -std::optional getIntersectionArea( +std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); +/** + * @fn + * @brief check if the given lane has related traffic light + */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); - -std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, - const double curvature_threshold, const double curvature_calculation_ds); - +/** + * @fn + * @brief interpolate PathWithLaneId + */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, @@ -125,56 +119,28 @@ std::optional generateInterpolatedPath( geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); -bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, - const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, - DebugData * debug_data); - -bool checkYieldStuckVehicleInIntersection( - const util::TargetObjects & target_objects, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, const std::string & turn_direction, - const double width, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, - DebugData * debug_data); - -Polygon2d generateStuckVehicleDetectAreaPolygon( - const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); - -std::optional checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double dist_margin, const bool is_parked_vehicle); - -void cutPredictPathWithDuration( - util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, - const double time_thr); +/** + * @fn + * @brief this function sorts the set of lanelets topologically using topological sort and merges + * the lanelets from each root to each end. each branch is merged and returned with the original + * lanelets + * @param[in] lanelets the set of lanelets + * @param[in] routing_graph_ptr the routing graph + * @return the pair of merged lanelets and their corresponding original lanelets + */ +std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr); -TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - -double calcDistanceUntilIntersectionLanelet( - const lanelet::ConstLanelet & assigned_lanelet, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx); - -lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval); - -std::optional generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::set & associative_ids, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx, - const double width); +/** + * @fn + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygon + */ +std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index d05031bf19436..763d829dacf9b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -34,178 +34,24 @@ namespace behavior_velocity_planner::util { -struct DebugData -{ - std::optional collision_stop_wall_pose{std::nullopt}; - std::optional occlusion_stop_wall_pose{std::nullopt}; - std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional pass_judge_wall_pose{std::nullopt}; - std::optional> attention_area{std::nullopt}; - std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; - std::optional> adjacent_area{std::nullopt}; - std::optional stuck_vehicle_detect_area{std::nullopt}; - std::optional> yield_stuck_detect_area{std::nullopt}; - std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - std::vector occlusion_polygons; - std::optional> - nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; - std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; -}; - +/** + * @struct + * @brief wrapper class of interpolated path with lane id + */ struct InterpolatedPathInfo { + /** the interpolated path */ autoware_auto_planning_msgs::msg::PathWithLaneId path; + /** discretization interval of interpolation */ double ds{0.0}; + /** the intersection lanelet id */ lanelet::Id lane_id{0}; + /** the associative lane ids of lane_id */ std::set associative_lane_ids{}; + /** the range of indices for the path points with associative lane id */ std::optional> lane_id_interval{std::nullopt}; }; -struct IntersectionLanelets -{ -public: - void update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); - const lanelet::ConstLanelets & attention() const - { - return is_prioritized_ ? attention_non_preceding_ : attention_; - } - const std::vector> & attention_stoplines() const - { - return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; - } - const lanelet::ConstLanelets & conflicting() const { return conflicting_; } - const lanelet::ConstLanelets & adjacent() const { return adjacent_; } - const lanelet::ConstLanelets & occlusion_attention() const - { - return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; - } - const std::vector & attention_area() const - { - return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; - } - const std::vector & conflicting_area() const - { - return conflicting_area_; - } - const std::vector & adjacent_area() const { return adjacent_area_; } - const std::vector & occlusion_attention_area() const - { - return occlusion_attention_area_; - } - const std::optional & first_conflicting_lane() const - { - return first_conflicting_lane_; - } - const std::optional & first_conflicting_area() const - { - return first_conflicting_area_; - } - const std::optional & first_attention_lane() const - { - return first_attention_lane_; - } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } - - lanelet::ConstLanelets attention_; // topologically merged lanelets - std::vector> - attention_stoplines_; // the stop lines for each attention_ lanelets - lanelet::ConstLanelets attention_non_preceding_; - std::vector> - attention_non_preceding_stoplines_; // the stop lines for each attention_non_preceding_ - // lanelets - lanelet::ConstLanelets conflicting_; - lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets - std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets - std::vector attention_area_; // topologically merged lanelets - std::vector attention_non_preceding_area_; - std::vector conflicting_area_; - std::vector adjacent_area_; - std::vector - occlusion_attention_area_; // topologically merged lanelets - // the first area intersecting with the path - // even if lane change/re-routing happened on the intersection, these areas area are supposed to - // be invariant under the 'associative' lanes. - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - bool is_prioritized_ = false; -}; - -struct IntersectionStopLines -{ - // NOTE: for baselink - size_t closest_idx{0}; - // NOTE: null if path does not conflict with first_conflicting_area - std::optional stuck_stopline{std::nullopt}; - // NOTE: null if path is over map stopline OR its value is calculated negative - std::optional default_stopline{std::nullopt}; - // NOTE: null if the index is calculated negative - std::optional first_attention_stopline{std::nullopt}; - // NOTE: null if footprints do not change from outside to inside of detection area - std::optional occlusion_peeking_stopline{std::nullopt}; - // if the value is calculated negative, its value is 0 - size_t pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -struct PathLanelets -{ - lanelet::ConstLanelets prev; - // lanelet::ConstLanelet entry2ego; this is included in `all` if exists - lanelet::ConstLanelet - ego_or_entry2exit; // this is `assigned lane` part of the path(not from - // ego) if ego is before the intersection, otherwise from ego to exit - std::optional next = - std::nullopt; // this is nullopt is the goal is inside intersection - lanelet::ConstLanelets all; - lanelet::ConstLanelets - conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the - // path -}; - -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - -enum class TrafficPrioritizedLevel { - // The target lane's traffic signal is red or the ego's traffic signal has an arrow. - FULLY_PRIORITIZED = 0, - // The target lane's traffic signal is amber - PARTIALLY_PRIORITIZED, - // The target lane's traffic signal is green - NOT_PRIORITIZED -}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ From 9244fea3c84823f3c828ba0021da9b8213359867 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 04:56:03 +0900 Subject: [PATCH 16/36] feat(intersection): distinguish 1st/2nd attention lanelet (#6042) --- .../src/debug.cpp | 16 ++ .../src/scene_intersection.cpp | 137 ++++++++++++++---- .../src/scene_intersection.hpp | 44 +++++- 3 files changed, 159 insertions(+), 38 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 83e218185a5ad..5f103e0ecd3b0 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -188,6 +188,22 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.first_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, + 0.0), + &debug_marker_array, now); + } + if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 292a6e6a8843d..35903d80c8273 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1008,7 +1008,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint, baselink2front); + intersection_lanelets.update( + is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); @@ -1019,6 +1020,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); + const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); // generate all stop line candidates // see the doc for struct IntersectionStopLines @@ -1029,16 +1031,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( : first_conflicting_lane; const auto intersection_stoplines_opt = generateIntersectionStopLines( - assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, interpolated_path_info, - path); + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, + interpolated_path_info, path); if (!intersection_stoplines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } const auto & intersection_stoplines = intersection_stoplines_opt.value(); - const auto - [closest_idx, stuck_stopline_idx_opt, default_stopline_idx_opt, - first_attention_stopline_idx_opt, occlusion_peeking_stopline_idx_opt, - default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; + const auto closest_idx = intersection_stoplines.closest_idx; + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; // see the doc for struct PathLanelets const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); @@ -1147,6 +1153,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.attention_area = intersection_lanelets.attention_area(); debug_data_.occlusion_attention_area = occlusion_attention_area; debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); // check occlusion on detection lane if (!occlusion_attention_divisions_) { @@ -1662,6 +1670,7 @@ IntersectionLanelets IntersectionModule::getObjectiveLanelets( std::optional IntersectionModule::generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, const util::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { @@ -1683,15 +1692,16 @@ std::optional IntersectionModule::generateIntersectionSto const int base2front_idx_dist = std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); - // find the index of the first point whose vehicle footprint on it intersects with detection_area + // find the index of the first point whose vehicle footprint on it intersects with attention_area const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - std::optional first_footprint_inside_detection_ip_opt = + const std::optional first_footprint_inside_1st_attention_ip_opt = getFirstPointInsidePolygonByFootprint( first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_detection_ip_opt) { + if (!first_footprint_inside_1st_attention_ip_opt) { return std::nullopt; } - const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); + const auto first_footprint_inside_1st_attention_ip = + first_footprint_inside_1st_attention_ip_opt.value(); std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { @@ -1719,7 +1729,7 @@ std::optional IntersectionModule::generateIntersectionSto stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; + stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stopline_valid = false; @@ -1735,7 +1745,7 @@ std::optional IntersectionModule::generateIntersectionSto // (3) occlusion peeking stop line position on interpolated path int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the detection area, invalid + // NOTE: if footprints[0] is already inside the attention area, invalid { const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( @@ -1747,32 +1757,32 @@ std::optional IntersectionModule::generateIntersectionSto } if (occlusion_peeking_line_valid) { occlusion_peeking_line_ip_int = - first_footprint_inside_detection_ip + std::ceil(peeking_offset / ds); + first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); } - const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; + + // (4) first attention stopline position on interpolated path + const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; const bool first_attention_stopline_valid = true; - // (4) pass judge line position on interpolated path + // (5) 1st pass judge line position on interpolated path const double velocity = planner_data_->current_velocity->twist.linear.x; const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( velocity, acceleration, max_accel, max_jerk, delay_response_time); - int pass_judge_ip_int = - static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); - const auto pass_judge_line_ip = static_cast( - std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - // TODO(Mamoru Sobue): maybe braking dist should be considered - const auto occlusion_wo_tl_pass_judge_line_ip = - static_cast(first_footprint_attention_centerline_ip); - - // (5) stuck vehicle stop line + int first_pass_judge_ip_int = + static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); + const auto first_pass_judge_line_ip = static_cast( + std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( + 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); + + // (6) stuck vehicle stopline position on interpolated path int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching detection area and already passed + // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); @@ -1791,14 +1801,37 @@ std::optional IntersectionModule::generateIntersectionSto } const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + // (7) second attention stopline position on interpolated path + int second_attention_stopline_ip_int = -1; + bool second_attention_stopline_valid = false; + if (second_attention_area_opt) { + const auto & second_attention_area = second_attention_area_opt.value(); + std::optional first_footprint_inside_2nd_attention_ip_opt = + getFirstPointInsidePolygonByFootprint( + second_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (first_footprint_inside_2nd_attention_ip_opt) { + second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + } + } + const auto second_attention_stopline_ip = + second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) + : 0; + + // (8) second pass judge lie position on interpolated path + int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); + const auto second_pass_judge_line_ip = + static_cast(std::max(second_pass_judge_ip_int, 0)); + struct IntersectionStopLinesTemp { size_t closest_idx{0}; size_t stuck_stopline{0}; size_t default_stopline{0}; size_t first_attention_stopline{0}; + size_t second_attention_stopline{0}; size_t occlusion_peeking_stopline{0}; - size_t pass_judge_line{0}; + size_t first_pass_judge_line{0}; + size_t second_pass_judge_line{0}; size_t occlusion_wo_tl_pass_judge_line{0}; }; @@ -1808,8 +1841,10 @@ std::optional IntersectionModule::generateIntersectionSto {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, + {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, + {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, {&occlusion_wo_tl_pass_judge_line_ip, &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; stoplines.sort( @@ -1843,11 +1878,17 @@ std::optional IntersectionModule::generateIntersectionSto intersection_stoplines.first_attention_stopline = intersection_stoplines_temp.first_attention_stopline; } + if (second_attention_stopline_valid) { + intersection_stoplines.second_attention_stopline = + intersection_stoplines_temp.second_attention_stopline; + } if (occlusion_peeking_line_valid) { intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } - intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; + intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -3252,7 +3293,8 @@ getFirstPointInsidePolygonsByFootprint( void IntersectionLanelets::update( const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path @@ -3265,13 +3307,44 @@ void IntersectionLanelets::update( } } if (!first_attention_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( + const auto first = getFirstPointInsidePolygonsByFootprint( attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); if (first) { first_attention_lane_ = attention_non_preceding_.at(first.value().second); first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } + if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { + const auto first_attention_lane = first_attention_lane_.value(); + // remove first_attention_area_ and non-straight lanelets from attention_non_preceding + lanelet::ConstLanelets attention_non_preceding_ex_first; + lanelet::ConstLanelets sibling_first_attention_lanelets; + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & following : routing_graph_ptr->following(previous)) { + sibling_first_attention_lanelets.push_back(following); + } + } + for (const auto & ll : attention_non_preceding_) { + // the sibling lanelets of first_attention_lanelet are ruled out + if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { + continue; + } + if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { + attention_non_preceding_ex_first.push_back(ll); + } + } + if (attention_non_preceding_ex_first.empty()) { + second_attention_lane_empty_ = true; + } + const auto attention_non_preceding_ex_first_area = + getPolygon3dFromLanelets(attention_non_preceding_ex_first); + const auto second = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); + if (second) { + second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); + second_attention_area_ = second_attention_lane_.value().polygon3d(); + } + } } void TargetObject::calc_dist_to_stopline() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6a350e66706e6..5d82ded78b9cb 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -53,6 +53,8 @@ struct DebugData std::optional> occlusion_attention_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; + std::optional first_attention_area{std::nullopt}; + std::optional second_attention_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; @@ -82,7 +84,8 @@ struct IntersectionLanelets */ void update( const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr); const lanelet::ConstLanelets & attention() const { @@ -131,6 +134,14 @@ struct IntersectionLanelets { return first_attention_area_; } + const std::optional & second_attention_lane() const + { + return second_attention_lane_; + } + const std::optional & second_attention_area() const + { + return second_attention_area_; + } /** * the set of attention lanelets which is topologically merged @@ -178,17 +189,25 @@ struct IntersectionLanelets std::vector occlusion_attention_size_; /** - * the attention lanelet which ego path points intersect for the first time + * the first conflicting lanelet which ego path points intersect for the first time */ std::optional first_conflicting_lane_{std::nullopt}; std::optional first_conflicting_area_{std::nullopt}; /** - * the attention lanelet which ego path points intersect for the first time + * the first attention lanelet which ego path points intersect for the first time */ std::optional first_attention_lane_{std::nullopt}; std::optional first_attention_area_{std::nullopt}; + /** + * the second attention lanelet which ego path points intersect next to the + * first_attention_lanelet + */ + bool second_attention_lane_empty_{false}; + std::optional second_attention_lane_{std::nullopt}; + std::optional second_attention_area_{std::nullopt}; + /** * flag if the intersection is prioritized by the traffic light */ @@ -219,16 +238,28 @@ struct IntersectionStopLines */ std::optional first_attention_stopline{std::nullopt}; + /** + * second_attention_stopline is null if ego footprint along the path does not intersect with + * second_attention_lane. if path[0] satisfies the condition, it is 0 + */ + std::optional second_attention_stopline{std::nullopt}; + /** * occlusion_peeking_stopline is null if path[0] is already inside the attention area */ std::optional occlusion_peeking_stopline{std::nullopt}; /** - * pass_judge_line is before first_attention_stop_line by the braking distance. if its value is - * calculated negative, it is 0 + * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value + * is calculated negative, it is 0 + */ + size_t first_pass_judge_line{0}; + + /** + * second_pass_judge_line is before second_attention_stopline by the braking distance. if its + * value is calculated negative, it is 0 */ - size_t pass_judge_line{0}; + size_t second_pass_judge_line{0}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with @@ -651,6 +682,7 @@ class IntersectionModule : public SceneModuleInterface lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, const util::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); From 6bd069f782cfaa406c21e5ac37439cb0c05cfaa4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 18:27:54 +0900 Subject: [PATCH 17/36] fix(intersection): fix bugs (#6050) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 +- planning/behavior_velocity_intersection_module/src/manager.cpp | 2 ++ .../src/scene_intersection.cpp | 2 +- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 6fc136c512689..4e9eb50f2a462 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -72,7 +72,7 @@ enable: false creep_velocity: 0.8333 peeking_offset: -0.5 - occlusion_required_clearance_distance: 55 + occlusion_required_clearance_distance: 55.0 possible_object_bbox: [1.5, 2.5] ignore_parked_vehicle_speed_threshold: 0.8333 occlusion_detection_hold_time: 1.5 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index e859b15b345b8..c92f16dd7b474 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -152,6 +152,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); ip.occlusion.peeking_offset = getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); ip.occlusion.possible_object_bbox = getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 35903d80c8273..c076b2a0d35ce 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1196,7 +1196,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } - const double is_amber_or_red = + const bool is_amber_or_red = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status = From 9102a13fedfce3ba99160d315849316304e663cb Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 18 Jan 2024 17:58:25 +0900 Subject: [PATCH 18/36] feat(intersection): more precise pass judge handling considering occlusion detection and 1st/2nd attention lane (#6047) Signed-off-by: Mamoru Sobue --- .../README.md | 86 +++- .../config/intersection.param.yaml | 2 +- .../docs/pass-judge-line.drawio.svg | 473 ++++++++++++++++++ .../src/debug.cpp | 20 +- .../src/manager.cpp | 4 +- .../src/scene_intersection.cpp | 106 ++-- .../src/scene_intersection.hpp | 16 +- 7 files changed, 634 insertions(+), 73 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 58c2ce59edd48..afb2381e628a4 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -88,7 +88,7 @@ To precisely calculate stop positions, the path is interpolated at the certain i - closest_idx denotes the path point index which is closest to ego position. - first_attention_stopline denotes the first path point where ego footprint intersects with the attention_area. -- If a stopline is associated with the intersection lane on the map, that line is used as the default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as the default_stopline instead. +- If a stopline is associated with the intersection lane on the map, that line is used as default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as default_stopline instead. - occlusion_peeking_stopline is a bit ahead of first_attention_stopline as described later. - occlusion_wo_tl_pass_judge_line is the first position where ego footprint intersects with the centerline of the first attention_area lane. @@ -113,8 +113,8 @@ There are several behaviors depending on the scene. | Safe | Ego detected no occlusion and collision | Ego passes the intersection | | StuckStop | The exit of the intersection is blocked by traffic jam | Ego stops before the intersection or the boundary of attention area | | YieldStuck | Another vehicle stops to yield ego | Ego stops before the intersection or the boundary of attention area | -| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at the default_stop_line | -| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at the default_stop_line at first | +| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at default_stopline | +| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at default_stopline at first | | PeekingTowardOcclusion | Ego detected occlusion and but no collision within the FOV (after FirstWaitBeforeOcclusion) | Ego approaches the boundary of the attention area slowly | | OccludedCollisionStop | Ego detected both occlusion and collision (after FirstWaitBeforeOcclusion) | Ego stops immediately | | FullyPrioritized | Ego is fully prioritized by the RED/Arrow signal | Ego only cares vehicles still running inside the intersection. Occlusion is ignored | @@ -219,7 +219,7 @@ ros2 run behavior_velocity_intersection_module ttc.py --lane_id ## Occlusion detection -If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough ego first makes a brief stop at the default stop line for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stop_line. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stop_line. Otherwise only stop line is inserted. +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough ego first makes a brief stop at default_stopline for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stopline. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stopline. Otherwise only stop line is inserted. During the creeping if collision is detected this module inserts a stop line in front of ego immediately, and if the FOV gets sufficiently clear the intersection_occlusion wall will disappear. If occlusion is cleared and no collision is detected ego will pass the intersection. @@ -241,7 +241,7 @@ The remaining time is visualized on the intersection_occlusion virtual wall. ### Occlusion handling at intersection without traffic light -At intersection without traffic light, if occlusion is detected, ego makes a brief stop at the default_stopline and first_attention_stopline respectively. After stopping at the first_attention_area_stopline this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and occlusion_wo_tl_pass_judge_line while occlusion is not cleared. If collision is detected, ego immediately stops. Once the occlusion is cleared or ego has passed occlusion_wo_tl_pass_judge_line this module does not detect collision and occlusion because ego footprint is already inside the intersection. +At intersection without traffic light, if occlusion is detected, ego makes a brief stop at default_stopline and first_attention_stopline respectively. After stopping at the first_attention_area_stopline this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and occlusion_wo_tl_pass_judge_line while occlusion is not cleared. If collision is detected, ego immediately stops. Once the occlusion is cleared or ego has passed occlusion_wo_tl_pass_judge_line this module does not detect collision and occlusion because ego footprint is already inside the intersection. ![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) @@ -263,7 +263,7 @@ TTC parameter varies depending on the traffic light color/shape as follows. ### yield on GREEN -If the traffic light color changed to GREEN and ego approached the entry of the intersection lane within the distance `collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start` and there is any object whose distance to its stopline is less than `collision_detection.yield_on_green_traffic_light.object_dist_to_stopline`, this module commands to stop for the duration of `collision_detection.yield_on_green_traffic_light.duration` at the default_stopline. +If the traffic light color changed to GREEN and ego approached the entry of the intersection lane within the distance `collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start` and there is any object whose distance to its stopline is less than `collision_detection.yield_on_green_traffic_light.object_dist_to_stopline`, this module commands to stop for the duration of `collision_detection.yield_on_green_traffic_light.duration` at default_stopline. ### skip on AMBER @@ -281,18 +281,49 @@ When the traffic light color/shape is RED/Arrow, occlusion detection is skipped. ## Pass Judge Line -To avoid sudden braking, if deceleration and jerk more than the threshold (`common.max_accel` and `common.max_jerk`) is required to stop at first_attention_stopline, this module does not command to stop once it passed the default_stopline position. +Generally it is not tolerable for vehicles that have lower traffic priority to stop in the middle of the unprotected area in intersections, and they need to stop at the stop line beforehand if there will be any risk of collision, which introduces two requirements: -If ego passed pass_judge_line, then ego does not stop anymore. If ego passed pass_judge_line while ego is stopping for dangerous decision, then ego stops while the situation is judged as dangerous. Once the judgement turned safe, ego restarts and does not stop anymore. +1. The vehicle must start braking before the boundary of the unprotected area at least by the braking distance if it is supposed to stop +2. The vehicle must recognize upcoming vehicles and check safety beforehand with enough braking distance margin if it is supposed to go + 1. And the SAFE decision must be absolutely certain and remain to be valid for the future horizon so that the safety condition will be always satisfied while ego is driving inside the unprotected area. +3. (TODO): Since it is almost impossible to make perfectly safe decision beforehand given the limited detection range/velocity tracking performance, intersection module should plan risk-evasive acceleration velocity profile AND/OR relax lateral acceleration limit while ego is driving inside the unprotected area, if the safety decision is "betrayed" later due to the following reasons: + 1. The situation _turned out to be dangerous_ later, mainly because velocity tracking was underestimated or the object accelerated beyond TTC margin + 2. The situation _turned dangerous_ later, mainly because the object is suddenly detected out of nowhere -The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane. +The position which is before the boundary of unprotected area by the braking distance which is obtained by -- If `occlusion.enable` is false, the pass judge line before the `first_attention_stopline` by the braking distance $v_{ego}^{2} / 2a_{max}$. -- If `occlusion.enable` is true and: - - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stopline` in order to continue peeking/collision detection while occlusion is detected. - - if there are no associated traffic lights and: - - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking. - - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false. +$$ +\dfrac{v_{\mathrm{ego}}^{2}}{2a_{\mathrm{max}}} + v_{\mathrm{ego}} * t_{\mathrm{delay}} +$$ + +is called pass_judge_line, and safety decision must be made before ego passes this position because ego does not stop anymore. + +1st_pass_judge_line is before the first upcoming lane, and at intersections with multiple upcoming lanes, 2nd_pass_judge_line is defined as the position which is before the centerline of the first attention lane by the braking distance. 1st/2nd_pass_judge_line are illustrated in the following figure. + +![pass-judge-line](./docs/pass-judge-line.drawio.svg) + +Intersection module will command to GO if + +- ego is over default_stopline(or `common.enable_pass_judge_before_default_stopline` is true) AND +- ego is over 1st_pass judge line AND +- ego judged SAFE previously AND +- (ego is over 2nd_pass_judge_line OR ego is between 1st and 2nd pass_judge_line but most probable collision is expected to happen in the 1st attention lane) + +because it is expected to stop or continue stop decision if + +1. ego is before default_stopline && `common.enable_pass_judge_before_default_stopline` is false OR + 1. reason: default_stopline is defined on the map and should be respected +2. ego is before 1st_pass_judge_line OR + 1. reason: it has enough braking distance margin +3. ego judged UNSAFE previously + 1. reason: ego is now trying to stop and should continue stop decision if collision is detected in later calculation +4. (ego is between 1st and 2nd pass_judge_line and the most probable collision is expected to happen in the 2nd attention lane) + +For the 3rd condition, it is possible that ego stops with some overshoot to the unprotected area while it is trying to stop for collision detection, because ego should keep stop decision while UNSAFE decision is made even if it passed 1st_pass_judge_line during deceleration. + +For the 4th condition, at intersections with 2nd attention lane, even if ego is over the 1st pass_judge_line, still intersection module commands to stop if the most probable collision is expected to happen in the 2nd attention lane. + +Also if `occlusion.enable` is true, the position of 1st_pass_judge line changes to occlusion_peeking_stopline if ego passed the original 1st_pass_judge_line position while ego is peeking. Otherwise ego could inadvertently judge that it passed 1st_pass_judge during peeking and then abort peeking. ## Data Structure @@ -375,17 +406,18 @@ entity TargetObject { ### common -| Parameter | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------------ | -| `.attention_area_length` | double | [m] range for object detection | -| `.attention_area_margin` | double | [m] margin for expanding attention area width | -| `.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | -| `.use_intersection_area` | bool | [-] flag to use intersection_area for collision detection | -| `.default_stopline_margin` | double | [m] margin before_stop_line | -| `.stopline_overshoot_margin` | double | [m] margin for the overshoot from stopline | -| `.max_accel` | double | [m/ss] max acceleration for stop | -| `.max_jerk` | double | [m/sss] max jerk for stop | -| `.delay_response_time` | double | [s] action delay before stop | +| Parameter | Type | Description | +| -------------------------------------------- | ------ | -------------------------------------------------------------------------------- | +| `.attention_area_length` | double | [m] range for object detection | +| `.attention_area_margin` | double | [m] margin for expanding attention area width | +| `.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | +| `.use_intersection_area` | bool | [-] flag to use intersection_area for collision detection | +| `.default_stopline_margin` | double | [m] margin before_stop_line | +| `.stopline_overshoot_margin` | double | [m] margin for the overshoot from stopline | +| `.max_accel` | double | [m/ss] max acceleration for stop | +| `.max_jerk` | double | [m/sss] max jerk for stop | +| `.delay_response_time` | double | [s] action delay before stop | +| `.enable_pass_judge_before_default_stopline` | bool | [-] flag not to stop before default_stopline even if ego is over pass_judge_line | ### stuck_vehicle/yield_stuck @@ -430,7 +462,7 @@ entity TargetObject { | `.possible_object_bbox` | [double] | [m] minimum bounding box size for checking if occlusion polygon is small enough | | `.ignore_parked_vehicle_speed_threshold` | double | [m/s] velocity threshold for checking parked vehicle | | `.occlusion_detection_hold_time` | double | [s] hold time of occlusion detection | -| `.temporal_stop_time_before_peeking` | double | [s] temporal stop duration at the default_stop_line before starting peeking | +| `.temporal_stop_time_before_peeking` | double | [s] temporal stop duration at default_stopline before starting peeking | | `.temporal_stop_before_attention_area` | bool | [-] flag to temporarily stop at first_attention_stopline before peeking into attention_area | | `.creep_velocity_without_traffic_light` | double | [m/s] creep velocity to occlusion_wo_tl_pass_judge_line | | `.static_occlusion_with_traffic_light_timeout` | double | [s] the timeout duration for ignoring static occlusion at intersection with traffic light | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 4e9eb50f2a462..108e021240851 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,7 @@ max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 + enable_pass_judge_before_default_stopline: false stuck_vehicle: turn_direction: @@ -36,7 +37,6 @@ consider_wrong_direction_vehicle: false collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 - keep_detection_velocity_threshold: 0.833 velocity_profile: use_upstream: true minimum_upstream_velocity: 0.01 diff --git a/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg new file mode 100644 index 0000000000000..d1f488af7c2ac --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg @@ -0,0 +1,473 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+ + 1st_pass_judge_line + + + + + + + + + + +
+
+
+ + 2nd_pass_judge_line_magin + +
+
+
+ + 2nd_pass_judge_line_magin + + + + + + + + + + +
+
+
+ + 2nd_pass_judge_line + +
+
+
+ + 2nd_pass_judge_line + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+ + 1st_pass_judge_line + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 5f103e0ecd3b0..350f6d774f7cf 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -90,7 +90,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( 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.scale = createMarkerScale(0.2, 0.0, 0.0); marker_line.color = createMarkerColor(r, g, b, 0.999); const double yaw = tf2::getYaw(pose.orientation); @@ -283,11 +283,23 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); */ - if (debug_data_.pass_judge_wall_pose) { + if (debug_data_.first_pass_judge_wall_pose) { + const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( createPoseMarkerArray( - debug_data_.pass_judge_wall_pose.value(), "pass_judge_wall_pose", module_id_, 0.7, 0.85, - 0.9), + debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, + g, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_pass_judge_wall_pose) { + const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; + appendMarkerArray( + createPoseMarkerArray( + debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, + r, g, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index c92f16dd7b474..0f9faaaa901c1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -61,6 +61,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); ip.common.delay_response_time = getOrDeclareParameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); ip.stuck_vehicle.turn_direction.left = getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); @@ -92,8 +94,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.collision_detection_hold_time"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); - ip.collision_detection.keep_detection_velocity_threshold = getOrDeclareParameter( - node, ns + ".collision_detection.keep_detection_velocity_threshold"); ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter(node, ns + ".collision_detection.velocity_profile.use_upstream"); ip.collision_detection.velocity_profile.minimum_upstream_velocity = getOrDeclareParameter( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index c076b2a0d35ce..a026fe8f4df31 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1042,7 +1042,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; - const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; @@ -1222,13 +1223,22 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( prev_occlusion_status_ = occlusion_status; } - // TODO(Mamoru Sobue): this part needs more formal handling - const size_t pass_judge_line_idx = [=]() { + const size_t pass_judge_line_idx = [&]() { if (enable_occlusion_detection_) { - // if occlusion detection is enabled, pass_judge position is beyond the boundary of first - // attention area if (has_traffic_light_) { - return occlusion_stopline_idx; + // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its + // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + if (passed_1st_judge_line_while_peeking_) { + return occlusion_stopline_idx; + } + const bool is_over_first_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, first_pass_judge_line_idx); + if (is_occlusion_state && is_over_first_pass_judge_line) { + passed_1st_judge_line_while_peeking_ = true; + return occlusion_stopline_idx; + } else { + return first_pass_judge_line_idx; + } } else if (is_occlusion_state) { // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area @@ -1236,30 +1246,53 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } else { // if there is no traffic light and occlusion is not detected, pass_judge position is // default - return default_pass_judge_line_idx; + return first_pass_judge_line_idx; } } - return default_pass_judge_line_idx; + return first_pass_judge_line_idx; }(); - debug_data_.pass_judge_wall_pose = - planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); - const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - const double vel_norm = std::hypot( - planner_data_->current_velocity->twist.linear.x, - planner_data_->current_velocity->twist.linear.y); - const bool keep_detection = - (vel_norm < planner_param_.collision_detection.keep_detection_velocity_threshold); + const bool was_safe = std::holds_alternative(prev_decision_result_); - // if ego is over the pass judge line and not stopped - if (is_over_default_stopline && !is_over_pass_judge_line && keep_detection) { - RCLCPP_DEBUG(logger_, "is_over_default_stopline && !is_over_pass_judge_line && keep_detection"); - // do nothing - } else if ( - (was_safe && is_over_default_stopline && is_over_pass_judge_line && is_go_out_) || + + const bool is_over_1st_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); + if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { + safely_passed_1st_judge_line_time_ = clock_->now(); + } + debug_data_.first_pass_judge_wall_pose = + planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); + debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); + const bool is_over_2nd_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, second_pass_judge_line_idx); + if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = clock_->now(); + } + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, *path); + debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); + + if ( + ((is_over_default_stopline || + planner_param_.common.enable_pass_judge_before_default_stopline) && + is_over_2nd_pass_judge_line && was_safe) || is_permanent_go_) { - // is_go_out_: previous RTC approval - // activated_: current RTC approval + /* + * This body is active if ego is + * - over the default stopline AND + * - over the 1st && 2nd pass judge line AND + * - previously safe + * , + * which means ego can stop even if it is over the 1st pass judge line but + * - before default stopline OR + * - before the 2nd pass judge line OR + * - or previously unsafe + * . + * In order for ego to continue peeking or collision detection when occlusion is detected after + * ego passed the 1st pass judge line, it needs to be + * - before the default stopline OR + * - before the 2nd pass judge line OR + * - previously unsafe + */ is_permanent_go_ = true; return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } @@ -1311,10 +1344,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( : (planner_param_.collision_detection.collision_detection_hold_time - collision_state_machine_.getDuration()); + // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd + // pass judge line respectively, ego should go + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, - std::min(occlusion_stopline_idx, path->points.size() - 1), time_to_restart, - traffic_prioritized_level); + *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, + time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1811,16 +1848,19 @@ std::optional IntersectionModule::generateIntersectionSto second_attention_area, interpolated_path_info, local_footprint, baselink2front); if (first_footprint_inside_2nd_attention_ip_opt) { second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + second_attention_stopline_valid = true; } } const auto second_attention_stopline_ip = second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge lie position on interpolated path - int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); + // (8) second pass judge line position on interpolated path. It is the same as first pass judge + // line if second_attention_lane is null + int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; const auto second_pass_judge_line_ip = - static_cast(std::max(second_pass_judge_ip_int, 0)); + second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) + : first_pass_judge_line_ip; struct IntersectionStopLinesTemp { @@ -2806,8 +2846,6 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( return std::nullopt; } }(); - const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { const auto & p1 = smoothed_reference_path.points.at(i); @@ -2821,7 +2859,7 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; const double passing_velocity = [=]() { if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { + if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { return minimum_upstream_velocity; } return std::max(average_velocity, minimum_ego_velocity); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 5d82ded78b9cb..a1a7be6b0e551 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -48,7 +48,10 @@ struct DebugData std::optional collision_stop_wall_pose{std::nullopt}; std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional pass_judge_wall_pose{std::nullopt}; + std::optional first_pass_judge_wall_pose{std::nullopt}; + bool passed_first_pass_judge{false}; + bool passed_second_pass_judge{false}; + std::optional second_pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; std::optional ego_lane{std::nullopt}; @@ -256,8 +259,8 @@ struct IntersectionStopLines size_t first_pass_judge_line{0}; /** - * second_pass_judge_line is before second_attention_stopline by the braking distance. if its - * value is calculated negative, it is 0 + * second_pass_judge_line is before second_attention_stopline by the braking distance. if + * second_attention_lane is null, it is same as first_pass_judge_line */ size_t second_pass_judge_line{0}; @@ -344,6 +347,7 @@ class IntersectionModule : public SceneModuleInterface double max_accel; double max_jerk; double delay_response_time; + bool enable_pass_judge_before_default_stopline; } common; struct TurnDirection @@ -373,7 +377,6 @@ class IntersectionModule : public SceneModuleInterface bool consider_wrong_direction_vehicle; double collision_detection_hold_time; double min_predicted_path_confidence; - double keep_detection_velocity_threshold; struct VelocityProfile { bool use_upstream; @@ -606,7 +609,7 @@ class IntersectionModule : public SceneModuleInterface private: rclcpp::Node & node_; - const int64_t lane_id_; + const lanelet::Id lane_id_; const std::set associative_ids_; const std::string turn_direction_; const bool has_traffic_light_; @@ -621,6 +624,9 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; OcclusionType prev_occlusion_status_; + bool passed_1st_judge_line_while_peeking_{false}; + std::optional safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; From 5036d19aacb7a213cf44e78ac74af58e6a292da3 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 11 Jan 2024 03:35:07 +0900 Subject: [PATCH 19/36] feat(behavior_velocity): add the option to keep the last valid observation (#6036) * feat(behavior_velocity): add the option to keep the last valid observation Signed-off-by: Mamoru Sobue * keep_last_observation is false by default and intersection/traffic_light is explicily true Signed-off-by: Mamoru Sobue * for intersection Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../src/scene_crosswalk.cpp | 9 +++--- .../src/scene_intersection.cpp | 31 +++++++++---------- .../src/scene_intersection.hpp | 9 ++++-- .../behavior_velocity_planner/src/node.cpp | 16 +++++++++- .../planner_data.hpp | 19 +++++++++--- .../src/scene.cpp | 8 ++--- 6 files changed, 60 insertions(+), 32 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index c2893ce7d1f92..169d42d7955e5 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1035,19 +1035,20 @@ bool CrosswalkModule::isRedSignalForPedestrians() const crosswalk_.regulatoryElementsAs(); for (const auto & traffic_lights_reg_elem : traffic_lights_reg_elems) { - const auto traffic_signal_stamped = + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal(traffic_lights_reg_elem->id()); - if (!traffic_signal_stamped) { + if (!traffic_signal_stamped_opt) { continue; } + const auto traffic_signal_stamped = traffic_signal_stamped_opt.value(); if ( planner_param_.traffic_light_state_timeout < - (clock_->now() - traffic_signal_stamped->stamp).seconds()) { + (clock_->now() - traffic_signal_stamped.stamp).seconds()) { continue; } - const auto & lights = traffic_signal_stamped->signal.elements; + const auto & lights = traffic_signal_stamped.signal.elements; if (lights.empty()) { continue; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index a026fe8f4df31..358dca2414bb0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -940,12 +940,11 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } -static bool isGreenSolidOn( - lanelet::ConstLanelet lane, const std::map & tl_infos) +bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; @@ -954,12 +953,13 @@ static bool isGreenSolidOn( // this lane has no traffic light return false; } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { // the info of this traffic light is not available return false; } - const auto & tl_info = tl_info_it->second; + const auto & tl_info = tl_info_opt.value(); for (auto && tl_light : tl_info.signal.elements) { if ( tl_light.color == TrafficSignalElement::GREEN && @@ -1004,8 +1004,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // at the very first time of regisTration of this module, the path may not be conflicting with the // attention area, so update() is called to update the internal data as well as traffic light info - const auto traffic_prioritized_level = - getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); const bool is_prioritized = traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; intersection_lanelets.update( @@ -1300,8 +1299,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // If there are any vehicles on the attention area when ego entered the intersection on green // light, do pseudo collision detection because the vehicles are very slow and no collisions may // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = - isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet); if (is_green_solid_on) { if (!initial_green_light_observed_time_) { const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); @@ -1458,12 +1456,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } -TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos) +TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - std::optional tl_id = std::nullopt; + std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); break; @@ -1472,12 +1469,12 @@ TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel( // this lane has no traffic light return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto tl_info_it = tl_infos.find(tl_id.value()); - if (tl_info_it == tl_infos.end()) { - // the info of this traffic light is not available + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & tl_info = tl_info_it->second; + const auto & tl_info = tl_info_opt.value(); bool has_amber_signal{false}; for (auto && tl_light : tl_info.signal.elements) { if (tl_light.color == TrafficSignalElement::AMBER) { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index a1a7be6b0e551..1a9cf74624fc0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -668,8 +668,7 @@ class IntersectionModule : public SceneModuleInterface * @fn * @brief find TrafficPrioritizedLevel */ - TrafficPrioritizedLevel getTrafficPrioritizedLevel( - lanelet::ConstLanelet lane, const std::map & tl_infos); + TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane); /** * @fn @@ -776,6 +775,12 @@ class IntersectionModule : public SceneModuleInterface const std::vector & lane_divisions, const TargetObjects & target_objects); + /* + * @fn + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn(lanelet::ConstLanelet lane); + /* bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index b5d019820bebe..04bbea741b181 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -334,7 +334,21 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + const bool is_unknown_observation = + std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { + return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + }); + // if the observation is UNKNOWN and past observation is available, only update the timestamp + // and keep the body of the info + if ( + is_unknown_observation && + planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + msg->stamp; + } else { + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + } } } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index d1f0c39c71205..9a7af95128c0c 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -77,7 +77,10 @@ struct PlannerData double ego_nearest_yaw_threshold; // other internal data - std::map traffic_light_id_map; + // traffic_light_id_map_raw is the raw observation, while traffic_light_id_map_keep_last keeps the + // last observed infomation for UNKNOWN + std::map traffic_light_id_map_raw_; + std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; std::map traffic_light_time_to_red_id_map; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; @@ -131,12 +134,20 @@ struct PlannerData return true; } - std::shared_ptr getTrafficSignal(const int id) const + /** + *@fn + *@brief queries the traffic signal information of given Id. if keep_last_observation = true, + *recent UNKNOWN observation is overwritten as the last non-UNKNOWN observation + */ + std::optional getTrafficSignal( + const lanelet::Id id, const bool keep_last_observation = false) const { + const auto & traffic_light_id_map = + keep_last_observation ? traffic_light_id_map_last_observed_ : traffic_light_id_map_raw_; if (traffic_light_id_map.count(id) == 0) { - return {}; + return std::nullopt; } - return std::make_shared(traffic_light_id_map.at(id)); + return std::make_optional(traffic_light_id_map.at(id)); } std::optional getRestTimeToRedSignal(const int id) const diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 83443bc726bbd..adc1aacf94f48 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -413,15 +413,15 @@ bool TrafficLightModule::isTrafficSignalStop( bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_traffic_signal) const { // get traffic signal associated with the regulatory element id - const auto traffic_signal_stamped = planner_data_->getTrafficSignal(traffic_light_reg_elem_.id()); - if (!traffic_signal_stamped) { + const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( + traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); + if (!traffic_signal_stamped_opt) { RCLCPP_WARN_THROTTLE( logger_, *clock_, 5000 /* ms */, "the traffic signal data associated with regulatory element id is not received"); return false; } - - valid_traffic_signal = *traffic_signal_stamped; + valid_traffic_signal = traffic_signal_stamped_opt.value(); return true; } From a0b82de65a00e2df541aeda78687bbdd9fc9e66a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 23 Jan 2024 03:02:28 +0900 Subject: [PATCH 20/36] refactor(intersection): divide source files and modifyPathVelocity (#6134) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 9 +- .../src/decision_result.hpp | 203 ++ ...il_type.hpp => interpolated_path_info.hpp} | 20 +- .../src/intersection_lanelets.cpp | 82 + .../src/intersection_lanelets.hpp | 197 ++ .../src/intersection_stoplines.hpp | 78 + .../src/manager.cpp | 5 +- .../src/manager.hpp | 1 - .../src/result.hpp | 44 + .../src/scene_intersection.cpp | 2995 +++-------------- .../src/scene_intersection.hpp | 884 +++-- .../src/scene_intersection_collision.cpp | 582 ++++ .../src/scene_intersection_occlusion.cpp | 407 +++ .../src/scene_intersection_prepare_data.cpp | 869 +++++ .../src/scene_intersection_stuck.cpp | 380 +++ .../src/scene_merge_from_private_road.cpp | 9 +- .../src/util.cpp | 58 +- .../src/util.hpp | 37 +- 18 files changed, 3794 insertions(+), 3066 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/src/decision_result.hpp rename planning/behavior_velocity_intersection_module/src/{util_type.hpp => interpolated_path_info.hpp} (71%) create mode 100644 planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp create mode 100644 planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp create mode 100644 planning/behavior_velocity_intersection_module/src/result.hpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 9e7eb196cd0c1..4c8fe5c6fa0f5 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -8,11 +8,16 @@ pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) find_package(OpenCV REQUIRED) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp src/manager.cpp + src/util.cpp src/scene_intersection.cpp + src/intersection_lanelets.cpp + src/scene_intersection_prepare_data.cpp + src/scene_intersection_stuck.cpp + src/scene_intersection_occlusion.cpp + src/scene_intersection_collision.cpp src/scene_merge_from_private_road.cpp - src/util.cpp + src/debug.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp new file mode 100644 index 0000000000000..48b0ecf1349a5 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -0,0 +1,203 @@ +// Copyright 2024 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 DECISION_RESULT_HPP_ +#define DECISION_RESULT_HPP_ + +#include +#include +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief Internal error or ego already passed pass_judge_line + */ +struct Indecisive +{ + std::string error; +}; + +/** + * @struct + * @brief detected stuck vehicle + */ +struct StuckStop +{ + size_t closest_idx{0}; + size_t stuck_stopline_idx{0}; + std::optional occlusion_stopline_idx{std::nullopt}; +}; + +/** + * @struct + * @brief yielded by vehicle on the attention area + */ +struct YieldStuckStop +{ + size_t closest_idx{0}; + size_t stuck_stopline_idx{0}; +}; + +/** + * @struct + * @brief only collision is detected + */ +struct NonOccludedCollisionStop +{ + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief occlusion is detected so ego needs to stop at the default stop line position + */ +struct FirstWaitBeforeOcclusion +{ + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief ego is approaching the boundary of attention area in the presence of traffic light + */ +struct PeekingTowardOcclusion +{ + //! if intersection_occlusion is disapproved externally through RTC, it indicates + //! "is_forcefully_occluded" + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck and shows up + //! intersection_occlusion(x.y) + std::optional static_occlusion_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief both collision and occlusion are detected in the presence of traffic light + */ +struct OccludedCollisionStop +{ + bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; + //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it + //! contains the remaining time to release the static occlusion stuck + std::optional static_occlusion_timeout{std::nullopt}; +}; + +/** + * @struct + * @brief at least occlusion is detected in the absence of traffic light + */ +struct OccludedAbsenceTrafficLight +{ + bool is_actually_occlusion_cleared{false}; + bool collision_detected{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t first_attention_area_stopline_idx{0}; + size_t peeking_limit_line_idx{0}; +}; + +/** + * @struct + * @brief both collision and occlusion are not detected + */ +struct Safe +{ + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +/** + * @struct + * @brief traffic light is red or arrow signal + */ +struct FullyPrioritized +{ + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; +}; + +using DecisionResult = std::variant< + Indecisive, //! internal process error, or over the pass judge line + StuckStop, //! detected stuck vehicle + YieldStuckStop, //! detected yield stuck vehicle + NonOccludedCollisionStop, //! detected collision while FOV is clear + FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion + PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected + OccludedCollisionStop, //! occlusion and collision are both detected + OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light + Safe, //! judge as safe + FullyPrioritized //! only detect vehicles violating traffic rules + >; + +inline std::string formatDecisionResult(const DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto indecisive = std::get(decision_result); + return "Indecisive because " + indecisive.error; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "YieldStuckStop"; + } + if (std::holds_alternative(decision_result)) { + return "NonOccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedCollisionStop"; + } + if (std::holds_alternative(decision_result)) { + return "OccludedAbsenceTrafficLight"; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + return "FullyPrioritized"; + } + return ""; +} + +} // namespace behavior_velocity_planner::intersection + +#endif // DECISION_RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp similarity index 71% rename from planning/behavior_velocity_intersection_module/src/util_type.hpp rename to planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index 763d829dacf9b..c47f016fbdbda 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -12,26 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef UTIL_TYPE_HPP_ -#define UTIL_TYPE_HPP_ +#ifndef INTERPOLATED_PATH_INFO_HPP_ +#define INTERPOLATED_PATH_INFO_HPP_ -#include - -#include #include -#include -#include -#include -#include -#include +#include #include #include #include -#include -namespace behavior_velocity_planner::util +namespace behavior_velocity_planner::intersection { /** @@ -52,6 +44,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner::util +} // namespace behavior_velocity_planner::intersection -#endif // UTIL_TYPE_HPP_ +#endif // INTERPOLATED_PATH_INFO_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp new file mode 100644 index 0000000000000..555ea424dfef0 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -0,0 +1,82 @@ +// Copyright 2024 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 "intersection_lanelets.hpp" + +#include "util.hpp" + +#include +#include + +#include + +namespace behavior_velocity_planner::intersection +{ + +void IntersectionLanelets::update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + is_prioritized_ = is_prioritized; + // find the first conflicting/detection area polygon intersecting the path + if (!first_conflicting_area_) { + auto first = util::getFirstPointInsidePolygonsByFootprint( + conflicting_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); + } + } + if (!first_attention_area_) { + const auto first = util::getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); + if (first) { + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); + } + } + if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { + const auto first_attention_lane = first_attention_lane_.value(); + // remove first_attention_area_ and non-straight lanelets from attention_non_preceding + lanelet::ConstLanelets attention_non_preceding_ex_first; + lanelet::ConstLanelets sibling_first_attention_lanelets; + for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { + for (const auto & following : routing_graph_ptr->following(previous)) { + sibling_first_attention_lanelets.push_back(following); + } + } + for (const auto & ll : attention_non_preceding_) { + // the sibling lanelets of first_attention_lanelet are ruled out + if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { + continue; + } + if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { + attention_non_preceding_ex_first.push_back(ll); + } + } + if (attention_non_preceding_ex_first.empty()) { + second_attention_lane_empty_ = true; + } + const auto attention_non_preceding_ex_first_area = + util::getPolygon3dFromLanelets(attention_non_preceding_ex_first); + const auto second = util::getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); + if (second) { + second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); + second_attention_area_ = second_attention_lane_.value().polygon3d(); + } + } +} +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp new file mode 100644 index 0000000000000..11deae6bdc001 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -0,0 +1,197 @@ +// Copyright 2024 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 INTERSECTION_LANELETS_HPP_ +#define INTERSECTION_LANELETS_HPP_ + +#include "interpolated_path_info.hpp" + +#include + +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief see the document for more details of IntersectionLanelets + */ +struct IntersectionLanelets +{ +public: + /** + * update conflicting lanelets and traffic priority information + */ + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, + lanelet::routing::RoutingGraphPtr routing_graph_ptr); + + const lanelet::ConstLanelets & attention() const + { + return is_prioritized_ ? attention_non_preceding_ : attention_; + } + const std::vector> & attention_stoplines() const + { + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; + } + const lanelet::ConstLanelets & conflicting() const { return conflicting_; } + const lanelet::ConstLanelets & adjacent() const { return adjacent_; } + const lanelet::ConstLanelets & occlusion_attention() const + { + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; + } + const lanelet::ConstLanelets & attention_non_preceding() const + { + return attention_non_preceding_; + } + const std::vector & attention_area() const + { + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; + } + const std::vector & conflicting_area() const + { + return conflicting_area_; + } + const std::vector & adjacent_area() const { return adjacent_area_; } + const std::vector & occlusion_attention_area() const + { + return occlusion_attention_area_; + } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } + const std::optional & first_conflicting_area() const + { + return first_conflicting_area_; + } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } + const std::optional & first_attention_area() const + { + return first_attention_area_; + } + const std::optional & second_attention_lane() const + { + return second_attention_lane_; + } + const std::optional & second_attention_area() const + { + return second_attention_area_; + } + + /** + * the set of attention lanelets which is topologically merged + */ + lanelet::ConstLanelets attention_; + std::vector attention_area_; + + /** + * the stop lines for each attention_lanelets associated with traffic lights. At intersection + * without traffic lights, each value is null + */ + std::vector> attention_stoplines_; + + /** + * the conflicting part of attention lanelets + */ + lanelet::ConstLanelets attention_non_preceding_; + std::vector attention_non_preceding_area_; + + /** + * the stop lines for each attention_non_preceding_ + */ + std::vector> attention_non_preceding_stoplines_; + + /** + * the conflicting lanelets of the objective intersection lanelet + */ + lanelet::ConstLanelets conflicting_; + std::vector conflicting_area_; + + /** + * + */ + lanelet::ConstLanelets adjacent_; + std::vector adjacent_area_; + + /** + * the set of attention lanelets for occlusion detection which is topologically merged + */ + lanelet::ConstLanelets occlusion_attention_; + std::vector occlusion_attention_area_; + + /** + * the vector of sum of each occlusion_attention lanelet + */ + std::vector occlusion_attention_size_; + + /** + * the first conflicting lanelet which ego path points intersect for the first time + */ + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + + /** + * the first attention lanelet which ego path points intersect for the first time + */ + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; + + /** + * the second attention lanelet which ego path points intersect next to the + * first_attention_lanelet + */ + bool second_attention_lane_empty_{false}; + std::optional second_attention_lane_{std::nullopt}; + std::optional second_attention_area_{std::nullopt}; + + /** + * flag if the intersection is prioritized by the traffic light + */ + bool is_prioritized_{false}; +}; + +/** + * @struct + * @brief see the document for more details of PathLanelets + */ +struct PathLanelets +{ + lanelet::ConstLanelets prev; + // lanelet::ConstLanelet entry2ego; this is included in `all` if exists + lanelet::ConstLanelet + ego_or_entry2exit; // this is `assigned lane` part of the path(not from + // ego) if ego is before the intersection, otherwise from ego to exit + std::optional next = + std::nullopt; // this is nullopt is the goal is inside intersection + lanelet::ConstLanelets all; + lanelet::ConstLanelets + conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with + // conflicting lanelets plus the next lane part of the + // path +}; +} // namespace behavior_velocity_planner::intersection + +#endif // INTERSECTION_LANELETS_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp new file mode 100644 index 0000000000000..64d20b81e3fad --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -0,0 +1,78 @@ +// Copyright 2024 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 INTERSECTION_STOPLINES_HPP_ +#define INTERSECTION_STOPLINES_HPP_ + +#include + +namespace behavior_velocity_planner::intersection +{ + +/** + * @struct + * @brief see the document for more details of IntersectionStopLines + */ +struct IntersectionStopLines +{ + size_t closest_idx{0}; + + /** + * stuck_stopline is null if ego path does not intersect with first_conflicting_area + */ + std::optional stuck_stopline{std::nullopt}; + + /** + * default_stopline is null if it is calculated negative from first_attention_stopline + */ + std::optional default_stopline{std::nullopt}; + + /** + * first_attention_stopline is null if ego footprint along the path does not intersect with + * attention area. if path[0] satisfies the condition, it is 0 + */ + std::optional first_attention_stopline{std::nullopt}; + + /** + * second_attention_stopline is null if ego footprint along the path does not intersect with + * second_attention_lane. if path[0] satisfies the condition, it is 0 + */ + std::optional second_attention_stopline{std::nullopt}; + + /** + * occlusion_peeking_stopline is null if path[0] is already inside the attention area + */ + std::optional occlusion_peeking_stopline{std::nullopt}; + + /** + * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value + * is calculated negative, it is 0 + */ + size_t first_pass_judge_line{0}; + + /** + * second_pass_judge_line is before second_attention_stopline by the braking distance. if + * second_attention_lane is null, it is same as first_pass_judge_line + */ + size_t second_pass_judge_line{0}; + + /** + * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with + * the centerline of the first_attention_lane + */ + size_t occlusion_wo_tl_pass_judge_line{0}; +}; +} // namespace behavior_velocity_planner::intersection + +#endif // INTERSECTION_STOPLINES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0f9faaaa901c1..76ccedb7c23a8 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -181,8 +181,6 @@ void IntersectionModuleManager::launchNewModules( const auto lanelets = planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); // run occlusion detection only in the first intersection - // TODO(Mamoru Sobue): remove `enable_occlusion_detection` variable - const bool enable_occlusion_detection = intersection_param_.occlusion.enable; for (size_t i = 0; i < lanelets.size(); i++) { const auto ll = lanelets.at(i); const auto lane_id = ll.id(); @@ -212,8 +210,7 @@ void IntersectionModuleManager::launchNewModules( } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, - has_traffic_light, enable_occlusion_detection, node_, - logger_.get_child("intersection_module"), clock_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index ff9302db0b6af..88af4412e34eb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -44,7 +44,6 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC IntersectionModule::PlannerParam intersection_param_; // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - std::unordered_map occlusion_map_uuid_; void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp new file mode 100644 index 0000000000000..cc6ad880b8153 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -0,0 +1,44 @@ +// Copyright 2024 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 RESULT_HPP_ +#define RESULT_HPP_ + +#include + +namespace behavior_velocity_planner::intersection +{ + +template +class Result +{ +public: + static Result make_ok(const Ok & ok) { return Result(ok); } + static Result make_err(const Error & err) { return Result(err); } + +public: + explicit Result(const Ok & ok) : data_(ok) {} + explicit Result(const Error & err) : data_(err) {} + explicit operator bool() const noexcept { return std::holds_alternative(data_); } + bool operator!() const noexcept { return !static_cast(*this); } + const Ok & ok() const { return std::get(data_); } + const Error & err() const { return std::get(data_); } + +private: + std::variant data_; +}; + +} // namespace behavior_velocity_planner::intersection + +#endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 358dca2414bb0..90242dc3edd7e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,137 +16,42 @@ #include "util.hpp" -#include -#include +#include // for toGeomPoly #include -#include -#include -#include #include #include -#include -#include +#include // for toPolygon2d #include #include -#include -#include -#include +#include -#include -#include #include #include -#include +#include +#include #include #include -#include -#include #include -#include #include -namespace tier4_autoware_utils -{ - -template <> -inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) -{ - geometry_msgs::msg::Point point; - point.x = p.x(); - point.y = p.y(); - point.z = p.z(); - return point; -} - -} // namespace tier4_autoware_utils - namespace behavior_velocity_planner { namespace bg = boost::geometry; -namespace -{ -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - Polygon2d one_step_poly; - for (const auto & point : prev_poly.outer()) { - one_step_poly.outer().push_back(point); - } - for (const auto & point : next_poly.outer()) { - one_step_poly.outer().push_back(point); - } - - bg::correct(one_step_poly); - - Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - -static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - 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::TRAILER || - object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { - return true; - } - return false; -} - -static bool isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) -{ - 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::TRAILER || - 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; -} - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), has_traffic_light_(has_traffic_light), - enable_occlusion_detection_(enable_occlusion_detection), - occlusion_attention_divisions_(std::nullopt), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); @@ -185,6 +90,32 @@ IntersectionModule::IntersectionModule( "~/debug/intersection/object_ttc", 1); } +bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) +{ + debug_data_ = DebugData(); + *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); + + // set default RTC + initializeRTCStatus(); + + // calculate the + const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; + + const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + + intersection::formatDecisionResult(decision_result); + std_msgs::msg::String decision_result_msg; + decision_result_msg.data = decision_type; + decision_state_pub_->publish(decision_result_msg); + + prepareRTCStatus(decision_result, *path); + + reactRTCApproval(decision_result, path, stop_reason); + + RCLCPP_DEBUG(logger_, "===== plan end ====="); + return true; +} + void IntersectionModule::initializeRTCStatus() { setSafe(true); @@ -196,6 +127,255 @@ void IntersectionModule::initializeRTCStatus() // activated_ and occlusion_activated_ must be set from manager's RTC callback } +intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( + PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + const auto traffic_prioritized_level = getTrafficPrioritizedLevel(); + const bool is_prioritized = + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; + + const auto prepare_data = prepareIntersectionData(is_prioritized, path); + if (!prepare_data) { + return prepare_data.err(); + } + const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); + const auto & intersection_lanelets = intersection_lanelets_.value(); + + const auto closest_idx = intersection_stoplines.closest_idx; + + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; + + // stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const auto is_stuck_status = isStuckStatus(*path, intersection_stoplines, path_lanelets); + if (is_stuck_status) { + return is_stuck_status.value(); + } + + // if attention area is empty, collision/occlusion detection is impossible + if (!intersection_lanelets.first_attention_area()) { + return intersection::Indecisive{"attention area is empty"}; + } + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + if (!default_stopline_idx_opt) { + return intersection::Indecisive{"default stop line is null"}; + } + const auto default_stopline_idx = default_stopline_idx_opt.value(); + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the intersection + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { + return intersection::Indecisive{"occlusion stop line is null"}; + } + const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); + const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); + + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); + debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // get intersection area + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + // filter objects + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + const auto is_yield_stuck_status = + isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines, target_objects); + if (is_yield_stuck_status) { + return is_yield_stuck_status.value(); + } + + const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = + getOcclusionStatus( + traffic_prioritized_level, interpolated_path_info, intersection_lanelets, target_objects); + + // TODO(Mamoru Sobue): this should be called later for safety diagnosis + const auto is_over_pass_judge_lines_status = + isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); + if (is_over_pass_judge_lines_status) { + return is_over_pass_judge_lines_status.ok(); + } + [[maybe_unused]] const auto [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line] = + is_over_pass_judge_lines_status.err(); + + const auto & current_pose = planner_data_->current_odometry->pose; + const bool is_over_default_stopline = + util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; + + const auto is_green_pseudo_collision_status = isGreenPseudoCollisionStatus( + *path, collision_stopline_idx, intersection_stoplines, target_objects); + if (is_green_pseudo_collision_status) { + return is_green_pseudo_collision_status.value(); + } + + // if ego is waiting for collision detection, the entry time into the intersection is a bit + // delayed for the chattering hold + const bool is_go_out = (activated_ && occlusion_activated_); + const double time_to_restart = + (is_go_out || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); + + // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd + // pass judge line respectively, ego should go + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = calcIntersectionPassingTime( + *path, closest_idx, last_intersection_stopline_candidate_idx, time_to_restart, + &ego_ttc_time_array); + + const bool has_collision = checkCollision( + *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, + time_to_restart, traffic_prioritized_level); + collision_state_machine_.setStateWithMarginTime( + has_collision ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("collision state_machine"), *clock_); + const bool has_collision_with_margin = + collision_state_machine_.getState() == StateMachine::State::STOP; + + if (is_prioritized) { + return intersection::FullyPrioritized{ + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + + // Safe + if (!is_occlusion_state && !has_collision_with_margin) { + return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // Only collision + if (!is_occlusion_state && has_collision_with_margin) { + return intersection::NonOccludedCollisionStop{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // Occluded + // occlusion_status is assured to be not NOT_OCCLUDED + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; + const bool stopped_at_default_line = stoppedForDuration( + default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, + before_creep_state_machine_); + if (stopped_at_default_line) { + // if specified the parameter occlusion.temporal_stop_before_attention_area OR + // has_no_traffic_light_, ego will temporarily stop before entering attention area + const bool temporal_stop_before_attention_required = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? !stoppedForDuration( + first_attention_stopline_idx, + planner_param_.occlusion.temporal_stop_time_before_peeking, + temporal_stop_before_attention_state_machine_) + : false; + if (!has_traffic_light_) { + if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { + return intersection::Indecisive{ + "already passed maximum peeking line in the absence of traffic light"}; + } + return intersection::OccludedAbsenceTrafficLight{ + is_occlusion_cleared_with_margin, + has_collision_with_margin, + temporal_stop_before_attention_required, + closest_idx, + first_attention_stopline_idx, + occlusion_wo_tl_pass_judge_line_idx}; + } + // following remaining block is "has_traffic_light_" + // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; + const bool is_stuck_by_static_occlusion = + stoppedAtPosition( + occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && + is_static_occlusion; + if (has_collision_with_margin) { + // if collision is detected, timeout is reset + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } else if (is_stuck_by_static_occlusion) { + static_occlusion_timeout_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); + } + const bool release_static_occlusion_stuck = + (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); + if (!has_collision_with_margin && release_static_occlusion_stuck) { + return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED + const double max_timeout = + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + + planner_param_.occlusion.occlusion_detection_hold_time; + const std::optional static_occlusion_timeout = + is_stuck_by_static_occlusion + ? std::make_optional( + max_timeout - static_occlusion_timeout_state_machine_.getDuration() - + occlusion_stop_state_machine_.getDuration()) + : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); + if (has_collision_with_margin) { + return intersection::OccludedCollisionStop{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; + } else { + return intersection::PeekingTowardOcclusion{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; + } + } else { + const auto occlusion_stopline = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? first_attention_stopline_idx + : occlusion_stopline_idx; + return intersection::FirstWaitBeforeOcclusion{ + is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; + } +} + // template-specification based visitor pattern // https://en.cppreference.com/w/cpp/utility/variant/visit template @@ -218,7 +398,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const IntersectionModule::Indecisive & result, + [[maybe_unused]] const intersection::Indecisive & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -228,7 +408,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::StuckStop & result, + const intersection::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -248,7 +428,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::YieldStuckStop & result, + const intersection::YieldStuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { @@ -263,7 +443,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::NonOccludedCollisionStop & result, + const intersection::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -282,7 +462,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::FirstWaitBeforeOcclusion & result, + const intersection::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -301,7 +481,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::PeekingTowardOcclusion & result, + const intersection::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -320,7 +500,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::OccludedAbsenceTrafficLight & result, + const intersection::OccludedAbsenceTrafficLight & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -337,7 +517,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::OccludedCollisionStop & result, + const intersection::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -356,9 +536,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::Safe & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const intersection::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; @@ -375,7 +555,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const IntersectionModule::FullyPrioritized & result, + const intersection::FullyPrioritized & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -393,7 +573,7 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const DecisionResult & decision_result, + const intersection::DecisionResult & decision_result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; @@ -408,7 +588,7 @@ void IntersectionModule::prepareRTCStatus( setSafe(default_safety); setDistance(default_distance); occlusion_first_stop_required_ = - std::holds_alternative(decision_result); + std::holds_alternative(decision_result); } template @@ -416,7 +596,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); return; @@ -426,13 +606,13 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const IntersectionModule::Indecisive & decision_result, + [[maybe_unused]] const intersection::Indecisive & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, - [[maybe_unused]] DebugData * debug_data) + [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; } @@ -440,10 +620,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::StuckStop & decision_result, + const intersection::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -488,10 +669,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::YieldStuckStop & decision_result, + const intersection::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -520,10 +702,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::NonOccludedCollisionStop & decision_result, + const intersection::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -563,10 +746,10 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::FirstWaitBeforeOcclusion & decision_result, + const intersection::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -614,10 +797,10 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::PeekingTowardOcclusion & decision_result, + const intersection::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) + VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -671,10 +854,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::OccludedCollisionStop & decision_result, + const intersection::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -718,10 +902,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, + const intersection::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -771,10 +956,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::Safe & decision_result, + const intersection::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -813,10 +999,11 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::FullyPrioritized & decision_result, + const intersection::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason, VelocityFactorInterface * velocity_factor, DebugData * debug_data) + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -853,96 +1040,26 @@ void reactRTCApprovalByDecisionResult( return; } -void reactRTCApproval( - const bool rtc_default_approval, const bool rtc_occlusion_approval, - const IntersectionModule::DecisionResult & decision_result, - const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, - VelocityFactorInterface * velocity_factor, DebugData * debug_data) +void IntersectionModule::reactRTCApproval( + const intersection::DecisionResult & decision_result, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( - rtc_default_approval, rtc_occlusion_approval, decision, planner_param, baselink2front, path, - stop_reason, velocity_factor, debug_data); + activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, + stop_reason, &velocity_factor_, &debug_data_); }}, decision_result); return; } -static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) -{ - if (std::holds_alternative(decision_result)) { - const auto indecisive = std::get(decision_result); - return "Indecisive because " + indecisive.error; - } - if (std::holds_alternative(decision_result)) { - return "Safe"; - } - if (std::holds_alternative(decision_result)) { - return "StuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; - } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; - } - return ""; -} - -bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - debug_data_ = DebugData(); - *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - - // set default RTC - initializeRTCStatus(); - - // calculate the - const auto decision_result = modifyPathVelocityDetail(path, stop_reason); - prev_decision_result_ = decision_result; - - const std::string decision_type = - "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); - std_msgs::msg::String decision_result_msg; - decision_result_msg.data = decision_type; - decision_state_pub_->publish(decision_result_msg); - - prepareRTCStatus(decision_result, *path); - - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - reactRTCApproval( - activated_, occlusion_activated_, decision_result, planner_param_, baselink2front, path, - stop_reason, &velocity_factor_, &debug_data_); - - if (!activated_ || !occlusion_activated_) { - is_go_out_ = false; - } else { - is_go_out_ = true; - } - RCLCPP_DEBUG(logger_, "===== plan end ====="); - return true; -} - -bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) +bool IntersectionModule::isGreenSolidOn() const { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { @@ -970,260 +1087,134 @@ bool IntersectionModule::isGreenSolidOn(lanelet::ConstLanelet lane) return false; } -IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const { - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const std::string turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - const auto & current_pose = planner_data_->current_odometry->pose; - - // spline interpolation - const auto interpolated_path_info_opt = util::generateInterpolatedPath( - lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); - if (!interpolated_path_info_opt) { - return IntersectionModule::Indecisive{"splineInterpolate failed"}; - } - const auto & interpolated_path_info = interpolated_path_info_opt.value(); - if (!interpolated_path_info.lane_id_interval) { - return IntersectionModule::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}; - } + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - // cache intersection lane information because it is invariant - const auto lanelets_on_path = - planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); - if (!intersection_lanelets_) { - intersection_lanelets_ = - getObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path); - } - auto & intersection_lanelets = intersection_lanelets_.value(); + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & lane = lanelet_map_ptr->laneletLayer.get(lane_id_); - // at the very first time of regisTration of this module, the path may not be conflicting with the - // attention area, so update() is called to update the internal data as well as traffic light info - const auto traffic_prioritized_level = getTrafficPrioritizedLevel(assigned_lanelet); - const bool is_prioritized = - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update( - is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); - - // this is abnormal - const auto & conflicting_lanelets = intersection_lanelets.conflicting(); - const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); - const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); - if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { - return IntersectionModule::Indecisive{"conflicting area is empty"}; + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; } - const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); - const auto & first_conflicting_area = first_conflicting_area_opt.value(); - const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); - - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes - const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() - ? intersection_lanelets.first_attention_lane().value() - : first_conflicting_lane; - - const auto intersection_stoplines_opt = generateIntersectionStopLines( - assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, - interpolated_path_info, path); - if (!intersection_stoplines_opt) { - return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; + if (!tl_id) { + // this lane has no traffic light + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & intersection_stoplines = intersection_stoplines_opt.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; - const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; - const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; - const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; - const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; - const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; - const auto occlusion_wo_tl_pass_judge_line_idx = - intersection_stoplines.occlusion_wo_tl_pass_judge_line; - - // see the doc for struct PathLanelets - const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); - const auto & conflicting_area = intersection_lanelets.conflicting_area(); - const auto path_lanelets_opt = generatePathLanelets( - lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, - first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); - if (!path_lanelets_opt.has_value()) { - return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; + const auto tl_info_opt = planner_data_->getTrafficSignal( + tl_id.value(), true /* traffic light module keeps last observation*/); + if (!tl_info_opt) { + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } - const auto & path_lanelets = path_lanelets_opt.value(); - - // utility functions - auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); - }; - auto stoppedForDuration = - [&](const size_t pos, const double duration, StateMachine & state_machine) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < 0.0); - const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { - state_machine.setState(StateMachine::State::GO); - } - return state_machine.getState() == StateMachine::State::GO; - }; - auto stoppedAtPosition = [&](const size_t pos, const double duration) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; + const auto & tl_info = tl_info_opt.value(); + bool has_amber_signal{false}; + for (auto && tl_light : tl_info.signal.elements) { + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; } - return false; - }; - - // stuck vehicle detection is viable even if attention area is empty - // so this needs to be checked before attention area validation - const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets, &debug_data_); - const bool is_first_conflicting_lane_private = - (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); - if (stuck_detected) { - if ( - is_first_conflicting_lane_private && - planner_param_.stuck_vehicle.disable_against_private_lane) { - // do nothing - } else { - std::optional stopline_idx = std::nullopt; - if (stuck_stopline_idx_opt) { - const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < - -planner_param_.common.stopline_overshoot_margin; - if (!is_over_stuck_stopline) { - stopline_idx = stuck_stopline_idx_opt.value(); - } - } - if (!stopline_idx) { - if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { - stopline_idx = default_stopline_idx_opt.value(); - } else if ( - first_attention_stopline_idx_opt && - fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { - stopline_idx = closest_idx; - } - } - if (stopline_idx) { - return IntersectionModule::StuckStop{ - closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; - } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; } } - - // if attention area is empty, collision/occlusion detection is impossible - if (!first_attention_area_opt) { - return IntersectionModule::Indecisive{"attention area is empty"}; - } - const auto first_attention_area = first_attention_area_opt.value(); - - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line - if (!default_stopline_idx_opt) { - return IntersectionModule::Indecisive{"default stop line is null"}; - } - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection - if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return IntersectionModule::Indecisive{"occlusion stop line is null"}; + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; } - const auto default_stopline_idx = default_stopline_idx_opt.value(); - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); - const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); - const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); + return TrafficPrioritizedLevel::NOT_PRIORITIZED; +} +TargetObjects IntersectionModule::generateTargetObjects( + const intersection::IntersectionLanelets & intersection_lanelets, + const std::optional & intersection_area) const +{ + const auto & objects_ptr = planner_data_->predicted_objects; + // extract target objects + TargetObjects target_objects; + target_objects.header = objects_ptr->header; + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.occlusion_attention_area = occlusion_attention_area; - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - - // check occlusion on detection lane - if (!occlusion_attention_divisions_) { - occlusion_attention_divisions_ = generateDetectionLaneDivisions( - occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution); - } - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); - - // get intersection area - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); + for (const auto & object : objects_ptr->objects) { + // ignore non-vehicle type objects, such as pedestrian. + if (!isTargetCollisionVehicleType(object)) { + continue; + } - const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding(), - &debug_data_); - if (yield_stuck_detected) { - std::optional stopline_idx = std::nullopt; - const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; - const bool is_before_first_attention_stopline = - fromEgoDist(first_attention_stopline_idx) >= 0.0; - if (stuck_stopline_idx_opt) { - const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < - -planner_param_.common.stopline_overshoot_margin; - if (!is_over_stuck_stopline) { - stopline_idx = stuck_stopline_idx_opt.value(); - } + // check direction of objects + const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); + if (belong_adjacent_lanelet_id) { + continue; } - if (!stopline_idx) { - if (is_before_default_stopline) { - stopline_idx = default_stopline_idx; - } else if (is_before_first_attention_stopline) { - stopline_idx = closest_idx; + + const auto is_parked_vehicle = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + auto & container = is_parked_vehicle ? target_objects.parked_attention_objects + : target_objects.attention_objects; + if (intersection_area) { + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); + const auto intersection_area_2d = intersection_area.value(); + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + if (belong_attention_lanelet_id) { + const auto id = belong_attention_lanelet_id.value(); + TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stopline = attention_lanelet_stoplines.at(id); + container.push_back(target_object); + } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { + TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = std::nullopt; + target_object.stopline = std::nullopt; + target_objects.intersection_area_objects.push_back(target_object); } - } - if (stopline_idx) { - return IntersectionModule::YieldStuckStop{closest_idx, stopline_idx.value()}; + } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( + object_direction, attention_lanelets, is_parked_vehicle); + belong_attention_lanelet_id.has_value()) { + // intersection_area is not available, use detection_area_with_margin as before + const auto id = belong_attention_lanelet_id.value(); + TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stopline = attention_lanelet_stoplines.at(id); + container.push_back(target_object); } } - - const bool is_amber_or_red = - (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || - (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); - auto occlusion_status = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? getOcclusionStatus( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) - : OcclusionType::NOT_OCCLUDED; - occlusion_stop_state_machine_.setStateWithMarginTime( - occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, - logger_.get_child("occlusion_stop"), *clock_); - const bool is_occlusion_cleared_with_margin = - (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // distinguish if ego detected occlusion or RTC detects occlusion - const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - if (ext_occlusion_requested) { - occlusion_status = OcclusionType::RTC_OCCLUDED; + for (const auto & object : target_objects.attention_objects) { + target_objects.all_attention_objects.push_back(object); } - const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); - if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { - occlusion_status = prev_occlusion_status_; - } else { - prev_occlusion_status_ = occlusion_status; + for (const auto & object : target_objects.parked_attention_objects) { + target_objects.all_attention_objects.push_back(object); } + for (auto & object : target_objects.all_attention_objects) { + object.calc_dist_to_stopline(); + } + return target_objects; +} +intersection::Result< + intersection::Indecisive, + std::pair> +IntersectionModule::isOverPassJudgeLinesStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const intersection::IntersectionStopLines & intersection_stoplines) +{ + const auto & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx = intersection_stoplines.closest_idx; + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto occlusion_wo_tl_pass_judge_line_idx = + intersection_stoplines.occlusion_wo_tl_pass_judge_line; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); const size_t pass_judge_line_idx = [&]() { - if (enable_occlusion_detection_) { + if (planner_param_.occlusion.enable) { if (has_traffic_light_) { // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line @@ -1231,7 +1222,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return occlusion_stopline_idx; } const bool is_over_first_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, first_pass_judge_line_idx); + util::isOverTargetIndex(path, closest_idx, current_pose, first_pass_judge_line_idx); if (is_occlusion_state && is_over_first_pass_judge_line) { passed_1st_judge_line_while_peeking_ = true; return occlusion_stopline_idx; @@ -1251,25 +1242,29 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return first_pass_judge_line_idx; }(); - const bool was_safe = std::holds_alternative(prev_decision_result_); + const bool was_safe = std::holds_alternative(prev_decision_result_); const bool is_over_1st_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); + util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { safely_passed_1st_judge_line_time_ = clock_->now(); } + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.first_pass_judge_wall_pose = - planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); + planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, path); debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); + const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; const bool is_over_2nd_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, second_pass_judge_line_idx); + util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx); if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { safely_passed_2nd_judge_line_time_ = clock_->now(); } debug_data_.second_pass_judge_wall_pose = - planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, *path); + planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path); debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); + const bool is_over_default_stopline = + util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); if ( ((is_over_default_stopline || planner_param_.common.enable_pass_judge_before_default_stopline) && @@ -1293,2093 +1288,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( * - previously unsafe */ is_permanent_go_ = true; - return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; - } - - // If there are any vehicles on the attention area when ego entered the intersection on green - // light, do pseudo collision detection because the vehicles are very slow and no collisions may - // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = isGreenSolidOn(assigned_lanelet); - if (is_green_solid_on) { - if (!initial_green_light_observed_time_) { - const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path->points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); - } - } - if (initial_green_light_observed_time_) { - const auto now = clock_->now(); - const bool exist_close_vehicles = std::any_of( - target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), - [&](const auto & object) { - return object.dist_to_stopline.has_value() && - object.dist_to_stopline.value() < - planner_param_.collision_detection.yield_on_green_traffic_light - .object_dist_to_stopline; - }); - if ( - exist_close_vehicles && - rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < - planner_param_.collision_detection.yield_on_green_traffic_light.duration) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - } - } - - // calculate dynamic collision around attention area - const double time_to_restart = - (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd - // pass judge line respectively, ego should go - const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; - const auto last_intersection_stopline_candidate_idx = - second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, - time_to_restart, traffic_prioritized_level); - collision_state_machine_.setStateWithMarginTime( - has_collision ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("collision state_machine"), *clock_); - const bool has_collision_with_margin = - collision_state_machine_.getState() == StateMachine::State::STOP; - - if (is_prioritized) { - return FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - - // Safe - if (!is_occlusion_state && !has_collision_with_margin) { - return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // Only collision - if (!is_occlusion_state && has_collision_with_margin) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED - const bool stopped_at_default_line = stoppedForDuration( - default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, - before_creep_state_machine_); - if (stopped_at_default_line) { - // if specified the parameter occlusion.temporal_stop_before_attention_area OR - // has_no_traffic_light_, ego will temporarily stop before entering attention area - const bool temporal_stop_before_attention_required = - (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) - ? !stoppedForDuration( - first_attention_stopline_idx, - planner_param_.occlusion.temporal_stop_time_before_peeking, - temporal_stop_before_attention_state_machine_) - : false; - if (!has_traffic_light_) { - if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return IntersectionModule::Indecisive{ - "already passed maximum peeking line in the absence of traffic light"}; - } - return IntersectionModule::OccludedAbsenceTrafficLight{ - is_occlusion_cleared_with_margin, - has_collision_with_margin, - temporal_stop_before_attention_required, - closest_idx, - first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; - } - // following remaining block is "has_traffic_light_" - // if ego is stuck by static occlusion in the presence of traffic light, start timeout count - const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; - const bool is_stuck_by_static_occlusion = - stoppedAtPosition( - occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && - is_static_occlusion; - if (has_collision_with_margin) { - // if collision is detected, timeout is reset - static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); - } else if (is_stuck_by_static_occlusion) { - static_occlusion_timeout_state_machine_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); - } - const bool release_static_occlusion_stuck = - (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); - if (!has_collision_with_margin && release_static_occlusion_stuck) { - return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; - } - // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED - const double max_timeout = - planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + - planner_param_.occlusion.occlusion_detection_hold_time; - const std::optional static_occlusion_timeout = - is_stuck_by_static_occlusion - ? std::make_optional( - max_timeout - static_occlusion_timeout_state_machine_.getDuration() - - occlusion_stop_state_machine_.getDuration()) - : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); - if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{ - is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stopline_idx, - first_attention_stopline_idx, - occlusion_stopline_idx, - static_occlusion_timeout}; - } else { - return IntersectionModule::PeekingTowardOcclusion{ - is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stopline_idx, - first_attention_stopline_idx, - occlusion_stopline_idx, - static_occlusion_timeout}; - } - } else { - const auto occlusion_stopline = - (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) - ? first_attention_stopline_idx - : occlusion_stopline_idx; - return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; - } -} - -TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel(lanelet::ConstLanelet lane) -{ - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - - std::optional tl_id = std::nullopt; - for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { - tl_id = tl_reg_elem->id(); - break; - } - if (!tl_id) { - // this lane has no traffic light - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto tl_info_opt = planner_data_->getTrafficSignal( - tl_id.value(), true /* traffic light module keeps last observation*/); - if (!tl_info_opt) { - return TrafficPrioritizedLevel::NOT_PRIORITIZED; - } - const auto & tl_info = tl_info_opt.value(); - bool has_amber_signal{false}; - for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color == TrafficSignalElement::AMBER) { - has_amber_signal = true; - } - if (tl_light.color == TrafficSignalElement::RED) { - // NOTE: Return here since the red signal has the highest priority. - return TrafficPrioritizedLevel::FULLY_PRIORITIZED; - } - } - if (has_amber_signal) { - return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; - } - return TrafficPrioritizedLevel::NOT_PRIORITIZED; -} - -static std::vector getPolygon3dFromLanelets( - const lanelet::ConstLanelets & ll_vec) -{ - std::vector polys; - for (auto && ll : ll_vec) { - polys.push_back(ll.polygon3d()); - } - return polys; -} - -IntersectionLanelets IntersectionModule::getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path) -{ - const double detection_area_length = planner_param_.common.attention_area_length; - const double occlusion_detection_area_length = - planner_param_.occlusion.occlusion_attention_area_length; - const bool consider_wrong_direction_vehicle = - planner_param_.collision_detection.consider_wrong_direction_vehicle; - - // retrieve a stopline associated with a traffic light - bool has_traffic_light = false; - if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); - tl_reg_elems.size() != 0) { - const auto tl_reg_elem = tl_reg_elems.front(); - const auto stopline_opt = tl_reg_elem->stopLine(); - if (!!stopline_opt) has_traffic_light = true; - } - - // for low priority lane - // If ego_lane has right of way (i.e. is high priority), - // ignore yieldLanelets (i.e. low priority lanes) - lanelet::ConstLanelets yield_lanelets{}; - 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); - } - } - } - } - - // get all following lanes of previous lane - lanelet::ConstLanelets ego_lanelets = lanelets_on_path; - 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 adjacent_followings; - - for (const auto & conflicting_lanelet : conflicting_lanelets) { - for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { - adjacent_followings.push_back(following_lanelet); - } - } - - // final objective lanelets - lanelet::ConstLanelets detection_lanelets; - lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stopline for stuck vehicle - for (auto && conflicting_lanelet : conflicting_lanelets) { - if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) - conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); - } - - // exclude yield lanelets and ego lanelets from detection_lanelets - if (turn_direction_ == std::string("straight") && has_traffic_light) { - // if assigned lanelet is "straight" with traffic light, detection area is not necessary - } else { - if (consider_wrong_direction_vehicle) { - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - for (const auto & adjacent_following : adjacent_followings) { - detection_lanelets.push_back(adjacent_following); - } - } else { - // otherwise we need to know the priority from RightOfWay - for (const auto & conflicting_lanelet : conflicting_lanelets) { - if ( - lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || - lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { - continue; - } - detection_lanelets.push_back(conflicting_lanelet); - } - } - } - - // get possible lanelet path that reaches conflicting_lane longer than given length - lanelet::ConstLanelets detection_and_preceding_lanelets; - { - const double length = detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) detection_and_preceding_lanelets.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, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) detection_and_preceding_lanelets.push_back(l); - } - } - } - } - - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; - { - const double length = occlusion_detection_area_length; - std::set detection_ids; - for (const auto & ll : detection_lanelets) { - // Preceding lanes does not include detection_lane so add them at the end - const auto & inserted = detection_ids.insert(ll.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.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, length, ego_lanelets); - for (const auto & ls : lanelet_sequences) { - for (const auto & l : ls) { - const auto & inserted = detection_ids.insert(l.id()); - if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); - } - } - } - } - lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; - for (const auto & ll : occlusion_detection_and_preceding_lanelets) { - const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (turn_direction == "left" || turn_direction == "right") { - continue; - } - occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); - } - - auto [attention_lanelets, original_attention_lanelet_sequences] = - util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - - IntersectionLanelets result; - result.attention_ = std::move(attention_lanelets); - for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { - // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that - // back() exists. - std::optional stopline{std::nullopt}; - for (auto it = original_attention_lanelet_seq.rbegin(); - it != original_attention_lanelet_seq.rend(); ++it) { - const auto traffic_lights = it->regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - break; - } - if (stopline) break; - } - result.attention_stoplines_.push_back(stopline); - } - result.attention_non_preceding_ = std::move(detection_lanelets); - for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stopline = std::nullopt; - const auto & ll = result.attention_non_preceding_.at(i); - const auto traffic_lights = ll.regulatoryElementsAs(); - for (const auto & traffic_light : traffic_lights) { - const auto stopline_opt = traffic_light->stopLine(); - if (!stopline_opt) continue; - stopline = stopline_opt.get(); - } - result.attention_non_preceding_stoplines_.push_back(stopline); - } - result.conflicting_ = std::move(conflicting_ex_ego_lanelets); - result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); - // NOTE: occlusion_attention is not inverted here - // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and - // then trim part of them based on curvature threshold - result.occlusion_attention_ = - std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); - - // NOTE: to properly update(), each element in conflicting_/conflicting_area_, - // attention_non_preceding_/attention_non_preceding_area_ need to be matched - result.attention_area_ = getPolygon3dFromLanelets(result.attention_); - result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); - result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); - result.adjacent_area_ = getPolygon3dFromLanelets(result.adjacent_); - result.occlusion_attention_area_ = getPolygon3dFromLanelets(result.occlusion_attention_); - return result; -} - -std::optional IntersectionModule::generateIntersectionStopLines( - lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, - const lanelet::ConstLanelet & first_attention_lane, - const std::optional & second_attention_area_opt, - const util::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) -{ - const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; - const double stopline_margin = planner_param_.common.default_stopline_margin; - const double max_accel = planner_param_.common.max_accel; - const double max_jerk = planner_param_.common.max_jerk; - const double delay_response_time = planner_param_.common.delay_response_time; - const double peeking_offset = planner_param_.occlusion.peeking_offset; - - const auto first_attention_area = first_attention_lane.polygon3d(); - const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); - const auto & path_ip = interpolated_path_info.path; - const double ds = interpolated_path_info.ds; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - - const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); - const int base2front_idx_dist = - std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / ds); - - // find the index of the first point whose vehicle footprint on it intersects with attention_area - const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); - const std::optional first_footprint_inside_1st_attention_ip_opt = - getFirstPointInsidePolygonByFootprint( - first_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (!first_footprint_inside_1st_attention_ip_opt) { - return std::nullopt; - } - const auto first_footprint_inside_1st_attention_ip = - first_footprint_inside_1st_attention_ip_opt.value(); - - std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; - for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { - // NOTE: maybe consideration of braking dist is necessary - first_footprint_attention_centerline_ip_opt = i; - break; - } - } - if (!first_footprint_attention_centerline_ip_opt) { - return std::nullopt; - } - const size_t first_footprint_attention_centerline_ip = - first_footprint_attention_centerline_ip_opt.value(); - - // (1) default stop line position on interpolated path - bool default_stopline_valid = true; - int stop_idx_ip_int = -1; - if (const auto map_stop_idx_ip = - getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); - map_stop_idx_ip) { - stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } - if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; - } - if (stop_idx_ip_int < 0) { - default_stopline_valid = false; - } - const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; - - // (2) ego front stop line position on interpolated path - const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; - const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( - path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - - // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); - bool occlusion_peeking_line_valid = true; - // NOTE: if footprints[0] is already inside the attention area, invalid - { - const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; - const auto path_footprint0 = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects( - path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { - occlusion_peeking_line_valid = false; - } - } - if (occlusion_peeking_line_valid) { - occlusion_peeking_line_ip_int = - first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); - } - const auto occlusion_peeking_line_ip = static_cast( - std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - - // (4) first attention stopline position on interpolated path - const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; - const bool first_attention_stopline_valid = true; - - // (5) 1st pass judge line position on interpolated path - const double velocity = planner_data_->current_velocity->twist.linear.x; - const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; - const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_accel, max_jerk, delay_response_time); - int first_pass_judge_ip_int = - static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); - const auto first_pass_judge_line_ip = static_cast( - std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( - 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); - - // (6) stuck vehicle stopline position on interpolated path - int stuck_stopline_ip_int = 0; - bool stuck_stopline_valid = true; - if (use_stuck_stopline) { - // NOTE: when ego vehicle is approaching attention area and already passed - // first_conflicting_area, this could be null. - const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stopline_idx_ip_opt) { - stuck_stopline_valid = false; - stuck_stopline_ip_int = 0; - } else { - stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; - } - } else { - stuck_stopline_ip_int = - std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); - } - if (stuck_stopline_ip_int < 0) { - stuck_stopline_valid = false; - } - const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); - - // (7) second attention stopline position on interpolated path - int second_attention_stopline_ip_int = -1; - bool second_attention_stopline_valid = false; - if (second_attention_area_opt) { - const auto & second_attention_area = second_attention_area_opt.value(); - std::optional first_footprint_inside_2nd_attention_ip_opt = - getFirstPointInsidePolygonByFootprint( - second_attention_area, interpolated_path_info, local_footprint, baselink2front); - if (first_footprint_inside_2nd_attention_ip_opt) { - second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); - second_attention_stopline_valid = true; - } - } - const auto second_attention_stopline_ip = - second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) - : 0; - - // (8) second pass judge line position on interpolated path. It is the same as first pass judge - // line if second_attention_lane is null - int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; - const auto second_pass_judge_line_ip = - second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) - : first_pass_judge_line_ip; - - struct IntersectionStopLinesTemp - { - size_t closest_idx{0}; - size_t stuck_stopline{0}; - size_t default_stopline{0}; - size_t first_attention_stopline{0}; - size_t second_attention_stopline{0}; - size_t occlusion_peeking_stopline{0}; - size_t first_pass_judge_line{0}; - size_t second_pass_judge_line{0}; - size_t occlusion_wo_tl_pass_judge_line{0}; - }; - - IntersectionStopLinesTemp intersection_stoplines_temp; - std::list> stoplines = { - {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, - {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, - {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, - {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, - {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, - {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, - {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, - {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, - {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; - stoplines.sort( - [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stoplines) { - const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; - const auto insert_idx = util::insertPointIndex( - insert_point, original_path, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); - if (!insert_idx) { - return std::nullopt; - } - *stop_idx = insert_idx.value(); - } - if ( - intersection_stoplines_temp.occlusion_peeking_stopline < - intersection_stoplines_temp.default_stopline) { - intersection_stoplines_temp.occlusion_peeking_stopline = - intersection_stoplines_temp.default_stopline; - } - - IntersectionStopLines intersection_stoplines; - intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; - if (stuck_stopline_valid) { - intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; - } - if (default_stopline_valid) { - intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; - } - if (first_attention_stopline_valid) { - intersection_stoplines.first_attention_stopline = - intersection_stoplines_temp.first_attention_stopline; - } - if (second_attention_stopline_valid) { - intersection_stoplines.second_attention_stopline = - intersection_stoplines_temp.second_attention_stopline; - } - if (occlusion_peeking_line_valid) { - intersection_stoplines.occlusion_peeking_stopline = - intersection_stoplines_temp.occlusion_peeking_stopline; - } - intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; - intersection_stoplines.second_pass_judge_line = - intersection_stoplines_temp.second_pass_judge_line; - intersection_stoplines.occlusion_wo_tl_pass_judge_line = - intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stoplines; -} - -/** - * @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. - */ -std::optional IntersectionModule::getStopLineIndexFromMap( - const util::InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) -{ - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - - const auto road_markings = - assigned_lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stopline; - for (const auto & road_marking : road_markings) { - const std::string type = - road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); - if (type == lanelet::AttributeValueString::StopLine) { - stopline.push_back(road_marking->roadMarking()); - break; // only one stopline exists. - } - } - if (stopline.empty()) { - return std::nullopt; - } - - const auto p_start = stopline.front().front(); - const auto p_end = stopline.front().back(); - const LineString2d extended_stopline = - planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); - - for (size_t i = lane_interval.first; i < lane_interval.second; 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_stopline, path_segment, collision_points); - - if (collision_points.empty()) { - continue; - } - - return i; - } - - geometry_msgs::msg::Pose stop_point_from_map; - stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); - stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); - stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); - - return motion_utils::findFirstNearestIndexWithSoftConstraints( - path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, - planner_data_->ego_nearest_yaw_threshold); -} - -static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) -{ - lanelet::ConstLanelets previous_lanelets; - for (const auto & ll : lanelets_on_path) { - if (associative_ids.find(ll.id()) != associative_ids.end()) { - return previous_lanelets; - } - previous_lanelets.push_back(ll); - } - return previous_lanelets; -} - -// end inclusive -static lanelet::ConstLanelet generatePathLanelet( - const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, - const double interval) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - size_t prev_idx = start_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto & p_prev = path.points.at(prev_idx).point.pose; - if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { - continue; - } - prev_idx = i; - const double yaw = tf2::getYaw(p.orientation); - const double x = p.position.x; - const double y = p.position.y; - // NOTE: maybe this is opposite - const double left_x = x + width / 2 * std::sin(yaw); - const double left_y = y - width / 2 * std::cos(yaw); - const double right_x = x - width / 2 * std::sin(yaw); - const double right_y = y + width / 2 * std::cos(yaw); - lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); - rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); - } - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); - - return lanelet::Lanelet(lanelet::InvalId, left, right); -} - -static std::optional> -getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, - const std::vector & polygons, const bool search_forward = true) -{ - if (search_forward) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - const 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) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - } - } else { - for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { - bool is_in_lanelet = false; - const 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) { - return std::make_optional>( - i, polygon); - } - } - if (is_in_lanelet) { - break; - } - if (i == 0) { - break; - } - } - } - return std::nullopt; -} - -std::optional IntersectionModule::generatePathLanelets( - const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::CompoundPolygon3d & first_conflicting_area, - const std::vector & conflicting_areas, - const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx) -{ - const double width = planner_data_->vehicle_info_.vehicle_width_m; - static constexpr double path_lanelet_interval = 1.5; - - const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; - if (!assigned_lane_interval_opt) { - return std::nullopt; - } - const auto assigned_lane_interval = assigned_lane_interval_opt.value(); - const auto & path = interpolated_path_info.path; - - PathLanelets path_lanelets; - // prev - path_lanelets.prev = getPrevLanelets(lanelets_on_path, associative_ids_); - path_lanelets.all = path_lanelets.prev; - - // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; - if (closest_idx > assigned_lane_start) { - path_lanelets.all.push_back( - generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); - } - - // ego_or_entry2exit - const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); - path_lanelets.ego_or_entry2exit = - generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); - - // next - if (assigned_lane_end < path.points.size() - 1) { - const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); - const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); - if (next_lane_interval_opt) { - const auto [next_start, next_end] = next_lane_interval_opt.value(); - path_lanelets.next = - generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); - path_lanelets.all.push_back(path_lanelets.next.value()); - } - } - - const auto first_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) - : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); - const auto last_inside_conflicting_idx_opt = - first_attention_area.has_value() - ? getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) - : getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); - if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { - const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); - const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; - lanelet::ConstLanelet conflicting_interval = generatePathLanelet( - path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, - path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); - if (last_inside_conflicting_idx < assigned_lane_end) { - lanelet::ConstLanelet remaining_interval = generatePathLanelet( - path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); - path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); - } - } - return path_lanelets; -} - -bool IntersectionModule::checkStuckVehicleInIntersection( - const PathLanelets & path_lanelets, DebugData * debug_data) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getLaneletLength3d; - using lanelet::utils::getPolygonFromArcLength; - using lanelet::utils::to2D; - - const bool stuck_detection_direction = [&]() { - return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || - (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || - (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); - }(); - if (!stuck_detection_direction) { - return false; - } - - const auto & objects_ptr = planner_data_->predicted_objects; - - // considering lane change in the intersection, these lanelets are generated from the path - const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; - Polygon2d stuck_vehicle_detect_area{}; - if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { - return false; - } - - double target_polygon_length = - getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); - lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; - if (path_lanelets.next) { - targets.push_back(path_lanelets.next.value()); - const double next_arc_length = - std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); - target_polygon_length += next_arc_length; - } - const auto target_polygon = - to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); - - if (target_polygon.empty()) { - return false; - } - - for (const auto & p : target_polygon) { - stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); - } - - stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); - bg::correct(stuck_vehicle_detect_area); - - debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - - for (const auto & object : objects_ptr->objects) { - if (!isTargetStuckVehicleType(object)) { - continue; // not target vehicle type - } - const auto obj_v_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { - continue; // not stop vehicle - } - - // check if the footprint is in the stuck detect area - const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); - const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); - if (is_in_stuck_area && debug_data) { - debug_data->stuck_targets.objects.push_back(object); - return true; - } - } - return false; -} - -static lanelet::LineString3d getLineStringFromArcLength( - const lanelet::ConstLineString3d & linestring, const double s1, const double s2) -{ - lanelet::Points3d points; - double accumulated_length = 0; - size_t start_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s1) { - start_index = i; - break; - } - accumulated_length += length; - } - if (start_index < linestring.size() - 1) { - const auto & p1 = linestring[start_index]; - const auto & p2 = linestring[start_index + 1]; - const double residue = s1 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto start_basic_point = p1.basicPoint() + residue * direction_vector; - const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); - points.push_back(start_point); - } - - accumulated_length = 0; - size_t end_index = linestring.size(); - for (size_t i = 0; i < linestring.size() - 1; i++) { - const auto & p1 = linestring[i]; - const auto & p2 = linestring[i + 1]; - const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); - if (accumulated_length + length > s2) { - end_index = i; - break; - } - accumulated_length += length; - } - - for (size_t i = start_index + 1; i < end_index; i++) { - const auto p = lanelet::Point3d(linestring[i]); - points.push_back(p); - } - if (end_index < linestring.size() - 1) { - const auto & p1 = linestring[end_index]; - const auto & p2 = linestring[end_index + 1]; - const double residue = s2 - accumulated_length; - const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); - const auto end_basic_point = p1.basicPoint() + residue * direction_vector; - const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); - points.push_back(end_point); - } - return lanelet::LineString3d{lanelet::InvalId, points}; -} - -static lanelet::ConstLanelet createLaneletFromArcLength( - const lanelet::ConstLanelet & lanelet, const double s1, const double s2) -{ - const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); - // make sure that s1, and s2 are between [0, lane_length] - const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); - const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); - - const auto ratio_s1 = s1_saturated / total_length; - const auto ratio_s2 = s2_saturated / total_length; - - const auto s1_left = - static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s2_left = - static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); - const auto s1_right = - static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); - const auto s2_right = - static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); - - const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); - const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); - - return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); -} - -bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data) -{ - const bool yield_stuck_detection_direction = [&]() { - return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || - (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || - (turn_direction_ == "straight" && planner_param_.yield_stuck.turn_direction.straight); - }(); - if (!yield_stuck_detection_direction) { - return false; - } - - const double width = planner_data_->vehicle_info_.vehicle_width_m; - const double stuck_vehicle_vel_thr = - planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; - const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; - - LineString2d sparse_intersection_path; - const auto [start, end] = interpolated_path_info.lane_id_interval.value(); - for (unsigned i = start; i < end; ++i) { - const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; - const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); - if (turn_direction_ == "right") { - const double right_x = point.x - width / 2 * std::sin(yaw); - const double right_y = point.y + width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(right_x, right_y); - } else if (turn_direction_ == "left") { - const double left_x = point.x + width / 2 * std::sin(yaw); - const double left_y = point.y - width / 2 * std::cos(yaw); - sparse_intersection_path.emplace_back(left_x, left_y); - } else { - // straight - sparse_intersection_path.emplace_back(point.x, point.y); - } - } - lanelet::ConstLanelets yield_stuck_detect_lanelets; - for (const auto & attention_lanelet : attention_lanelets) { - const auto centerline = attention_lanelet.centerline2d().basicLineString(); - std::vector intersects; - bg::intersection(sparse_intersection_path, centerline, intersects); - if (intersects.empty()) { - continue; - } - const auto intersect = intersects.front(); - const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( - centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); - const double yield_stuck_start = - std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); - const double yield_stuck_end = intersect_arc_coords.length; - yield_stuck_detect_lanelets.push_back( - createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); - } - debug_data->yield_stuck_detect_area = getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { - const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); - - if (obj_v_norm > stuck_vehicle_vel_thr) { - continue; - } - for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { - const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); - if (is_in_lanelet) { - debug_data->yield_stuck_targets.objects.push_back(object.object); - return true; - } - } - } - return false; -} - -TargetObjects IntersectionModule::generateTargetObjects( - const IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const -{ - const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects - TargetObjects target_objects; - target_objects.header = objects_ptr->header; - const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - for (const auto & object : objects_ptr->objects) { - // ignore non-vehicle type objects, such as pedestrian. - if (!isTargetCollisionVehicleType(object)) { - continue; - } - - // check direction of objects - const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = - checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); - if (belong_adjacent_lanelet_id) { - continue; - } - - const auto is_parked_vehicle = - std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; - auto & container = is_parked_vehicle ? target_objects.parked_attention_objects - : target_objects.attention_objects; - if (intersection_area) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - if (belong_attention_lanelet_id) { - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = std::nullopt; - target_object.stopline = std::nullopt; - target_objects.intersection_area_objects.push_back(target_object); - } - } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( - object_direction, attention_lanelets, is_parked_vehicle); - belong_attention_lanelet_id.has_value()) { - // intersection_area is not available, use detection_area_with_margin as before - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } - } - for (const auto & object : target_objects.attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stopline(); - } - return target_objects; -} - -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - - // check collision between target_objects predicted path and ego lane - // cut the predicted path at passing_time - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - ego_ttc_time_array.stamp = path.header.stamp; - ego_ttc_pub_->publish(ego_ttc_time_array); - } - - const double passing_time = time_distance_array.back().first; - cutPredictPathWithDuration(target_objects, passing_time); - - const auto & concat_lanelets = path_lanelets.all; - const auto closest_arc_coords = getArcCoordinates( - concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const auto & ego_lane = path_lanelets.ego_or_entry2exit; - debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - - // change TTC margin based on ego traffic light color - const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { - if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { - return std::make_pair( - planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, - planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); - } - if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { - return std::make_pair( - planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, - planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); - } - return std::make_pair( - planner_param_.collision_detection.not_prioritized.collision_start_margin_time, - planner_param_.collision_detection.not_prioritized.collision_end_margin_time); - }(); - const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { - if (!target_object.dist_to_stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - if (dist_to_stopline < 0) { - return false; - } - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - return dist_to_stopline > braking_distance; - }; - const auto isTolerableOvershoot = [&](const TargetObject & target_object) { - if ( - !target_object.attention_lanelet || !target_object.dist_to_stopline || - !target_object.stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - if (dist_to_stopline > braking_distance) { - return false; - } - const auto stopline_front = target_object.stopline.value().front(); - const auto stopline_back = target_object.stopline.value().back(); - tier4_autoware_utils::LineString2d object_line; - object_line.emplace_back( - (stopline_front.x() + stopline_back.x()) / 2.0, - (stopline_front.y() + stopline_back.y()) / 2.0); - const auto stopline_mid = object_line.front(); - const auto endpoint = target_object.attention_lanelet.value().centerline().back(); - object_line.emplace_back(endpoint.x(), endpoint.y()); - std::vector intersections; - bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); - if (intersections.empty()) { - return false; - } - const auto collision_point = intersections.front(); - // distance from object expected stop position to collision point - const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; - const double stopline_to_collision = - std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); - const double object2collision = stopline_to_collision - stopline_to_object; - const double margin = - planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; - return (object2collision > margin) || (object2collision < 0); - }; - // check collision between predicted_path and ego_area - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; - object_ttc_time_array.layout.dim.resize(3); - object_ttc_time_array.layout.dim.at(0).label = "objects"; - object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id - object_ttc_time_array.layout.dim.at(1).label = - "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " - "start_time, start_dist, " - "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " - "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; - object_ttc_time_array.layout.dim.at(1).size = 57; - for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { - object_ttc_time_array.data.push_back(lane_id_); - } - bool collision_detected = false; - for (const auto & target_object : target_objects->all_attention_objects) { - const auto & object = target_object.object; - // If the vehicle is expected to stop before their stopline, ignore - const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expected_to_stop_before_stopline) { - debug_data_.amber_ignore_targets.objects.push_back(object); - continue; - } - const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && - !expected_to_stop_before_stopline && is_tolerable_overshoot) { - debug_data_.red_overshoot_ignore_targets.objects.push_back(object); - continue; - } - for (const auto & predicted_path : object.kinematics.predicted_paths) { - if ( - predicted_path.confidence < - planner_param_.collision_detection.min_predicted_path_confidence) { - // ignore the predicted path with too low confidence - continue; - } - - // collision point - const auto first_itr = std::adjacent_find( - predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); - }); - if (first_itr == predicted_path.path.cend()) continue; - const auto last_itr = std::adjacent_find( - predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); - }); - if (last_itr == predicted_path.path.crend()) continue; - - // possible collision time interval - 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 - collision_start_margin_time > 0) { - // start of possible ego position in the intersection - start_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_enter_time - collision_start_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (start_time_distance_itr == time_distance_array.end()) { - // ego is already at the exit of intersection when npc is at collision point even if npc - // accelerates so ego's position interval is empty - 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 + collision_end_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (end_time_distance_itr == time_distance_array.end()) { - // ego is already passing the intersection, when npc is is at collision point - // so ego's position interval is up to the end of intersection lane - 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 - - planner_data_->vehicle_info_.rear_overhang_m); - const double end_arc_length = std::min( - closest_arc_coords.length + (*end_time_distance_itr).second + - planner_data_->vehicle_info_.max_longitudinal_offset_m, - lanelet::utils::getLaneletLength2d(concat_lanelets)); - - const auto trimmed_ego_polygon = - getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length); - - if (trimmed_ego_polygon.empty()) { - continue; - } - - Polygon2d polygon{}; - for (const auto & p : trimmed_ego_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - bg::correct(polygon); - debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); - - for (auto itr = first_itr; itr != last_itr.base(); ++itr) { - const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); - if (bg::intersects(polygon, footprint_polygon)) { - collision_detected = true; - break; - } - } - object_ttc_time_array.layout.dim.at(0).size++; - const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto & shape = object.shape; - object_ttc_time_array.data.insert( - object_ttc_time_array.data.end(), - {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), - shape.dimensions.x, shape.dimensions.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x, - 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, - start_time_distance_itr->first, start_time_distance_itr->second, - end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, - first_itr->position.y, last_itr->position.x, last_itr->position.y}); - for (unsigned i = 0; i < 20; i++) { - const auto & pos = - predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; - object_ttc_time_array.data.push_back(pos.x); - object_ttc_time_array.data.push_back(pos.y); - } - if (collision_detected) { - debug_data_.conflicting_targets.objects.push_back(object); - break; - } - } - } - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - object_ttc_time_array.stamp = path.header.stamp; - object_ttc_pub_->publish(object_ttc_time_array); - } - - return collision_detected; -} - -std::optional IntersectionModule::checkAngleForTargetLanelets( - - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const bool is_parked_vehicle) const -{ - const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; - const bool consider_wrong_direction_vehicle = - planner_param_.common.attention_area_angle_threshold; - const double dist_margin = planner_param_.common.attention_area_margin; - - for (unsigned i = 0; i < target_lanelets.size(); ++i) { - const auto & ll = target_lanelets.at(i); - if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { - continue; - } - const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); - const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); - if (consider_wrong_direction_vehicle) { - if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - } else { - if (std::fabs(angle_diff) < detection_area_angle_thr) { - return std::make_optional(i); - } - // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is - // positive - if ( - is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return std::make_optional(i); - } - } - } - return std::nullopt; -} - -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) -{ - const rclcpp::Time current_time = clock_->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.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(target_objects->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); - } - } - } - } -} - -TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) -{ - const double intersection_velocity = - planner_param_.collision_detection.velocity_profile.default_velocity; - const double minimum_ego_velocity = - planner_param_.collision_detection.velocity_profile.minimum_default_velocity; - const bool use_upstream_velocity = - planner_param_.collision_detection.velocity_profile.use_upstream; - const double minimum_upstream_velocity = - planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; - const double current_velocity = planner_data_->current_velocity->twist.linear.x; - - int assigned_lane_found = false; - - // crop intersection part of the path, and set the reference velocity to intersection_velocity - // for ego's ttc - PathWithLaneId reference_path; - std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - auto reference_point = path.points.at(i); - // assume backward velocity is current ego velocity - if (i < closest_idx) { - reference_point.point.longitudinal_velocity_mps = current_velocity; - } - if ( - i > last_intersection_stopline_candidate_idx && - std::fabs(reference_point.point.longitudinal_velocity_mps) < - std::numeric_limits::epsilon() && - !upstream_stopline) { - upstream_stopline = i; - } - if (!use_upstream_velocity) { - reference_point.point.longitudinal_velocity_mps = intersection_velocity; - } - reference_path.points.push_back(reference_point); - bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); - 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. - } - - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - - // apply smoother to reference velocity - PathWithLaneId smoothed_reference_path = reference_path; - if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { - smoothed_reference_path = reference_path; - } - - // calculate when ego is going to reach each (interpolated) points on the path - TimeDistanceArray time_distance_array{}; - double dist_sum = 0.0; - double passing_time = time_delay; - time_distance_array.emplace_back(passing_time, dist_sum); - - // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stopline_candidate_idx` makes no sense - const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, path.points.at(closest_idx).point.pose, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); - - const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { - if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; - return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stopline_point, - planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); - } else { - return std::nullopt; - } - }(); - - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { - const auto & p1 = smoothed_reference_path.points.at(i); - const auto & p2 = smoothed_reference_path.points.at(i + 1); - - const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); - dist_sum += dist; - - // use average velocity between p1 and p2 - const double average_velocity = - (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; - const double passing_velocity = [=]() { - if (use_upstream_velocity) { - if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { - return minimum_upstream_velocity; - } - return std::max(average_velocity, minimum_ego_velocity); - } else { - return std::max(average_velocity, minimum_ego_velocity); - } - }(); - passing_time += (dist / passing_velocity); - - time_distance_array.emplace_back(passing_time, dist_sum); - } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); - for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id_); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); - } - for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); - } - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { - const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); - } - return time_distance_array; -} - -static double getHighestCurvature(const lanelet::ConstLineString3d & centerline) -{ - std::vector points; - for (auto point = centerline.begin(); point != centerline.end(); point++) { - points.push_back(*point); - } - - SplineInterpolationPoints2d interpolation(points); - const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); - std::vector curvatures_positive; - for (const auto & curvature : curvatures) { - curvatures_positive.push_back(std::fabs(curvature)); - } - return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); -} - -std::vector IntersectionModule::generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) -{ - const double curvature_threshold = - planner_param_.occlusion.attention_lane_crop_curvature_threshold; - const double curvature_calculation_ds = - planner_param_.occlusion.attention_lane_curvature_calculation_ds; - - using lanelet::utils::getCenterlineWithOffset; - - // (0) remove left/right lanelet - lanelet::ConstLanelets detection_lanelets; - for (const auto & detection_lanelet : detection_lanelets_all) { - // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet - const auto fine_centerline = - lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); - const double highest_curvature = getHighestCurvature(fine_centerline); - if (highest_curvature > curvature_threshold) { - continue; - } - detection_lanelets.push_back(detection_lanelet); - } - - // (1) tsort detection_lanelets - const auto [merged_detection_lanelets, originals] = - util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); - - // (2) merge each branch to one lanelet - // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::vector> merged_lanelet_with_area; - for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { - const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); - const auto & original = originals.at(i); - double area = 0; - for (const auto & partition : original) { - area += bg::area(partition.polygon2d().basicPolygon()); - } - merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); - } - - // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { - const double length = bg::length(merged_lanelet.centerline()); - const double width = area / length; - for (int i = 0; i < static_cast(width / resolution); ++i) { - const double offset = resolution * i - width / 2; - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); - } - detection_divisions.push_back( - getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); - } - return detection_divisions; -} - -IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) -{ - const auto & occ_grid = *planner_data_->occupancy_grid; - const auto & current_pose = planner_data_->current_odometry->pose; - const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; - - const auto & path_ip = interpolated_path_info.path; - const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); - - const auto first_attention_area_idx = - util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); - if (!first_attention_area_idx) { - return OcclusionType::NOT_OCCLUDED; - } - - const auto first_inside_attention_idx_ip_opt = - util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); - const std::pair lane_attention_interval_ip = - first_inside_attention_idx_ip_opt - ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) - : lane_interval_ip; - const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; - - const int width = occ_grid.info.width; - const int height = occ_grid.info.height; - const double resolution = occ_grid.info.resolution; - const auto & origin = occ_grid.info.origin.position; - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - - Polygon2d grid_poly; - grid_poly.outer().emplace_back(origin.x, origin.y); - grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); - grid_poly.outer().emplace_back( - origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution); - grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution); - grid_poly.outer().emplace_back(origin.x, origin.y); - bg::correct(grid_poly); - - auto findCommonCvPolygons = - [&](const auto & area2d, std::vector> & cv_polygons) -> void { - tier4_autoware_utils::Polygon2d area2d_poly; - for (const auto & p : area2d) { - area2d_poly.outer().emplace_back(p.x(), p.y()); - } - area2d_poly.outer().push_back(area2d_poly.outer().front()); - bg::correct(area2d_poly); - std::vector common_areas; - bg::intersection(area2d_poly, grid_poly, common_areas); - if (common_areas.empty()) { - return; - } - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - for (const auto & common_area : common_areas) { - std::vector cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); - cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - cv_polygons.push_back(cv_polygon); - } - }; - - // (1) prepare detection area mask - // attention: 255 - // non-attention: 0 - // NOTE: interesting area is set to 255 for later masking - cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); - std::vector> attention_area_cv_polygons; - for (const auto & attention_area : attention_areas) { - const auto area2d = lanelet::utils::to2D(attention_area); - findCommonCvPolygons(area2d, attention_area_cv_polygons); - } - for (const auto & poly : attention_area_cv_polygons) { - cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); - } - // (1.1) - // reset adjacent_lanelets area to 0 on attention_mask - std::vector> adjacent_lane_cv_polygons; - for (const auto & adjacent_lanelet : adjacent_lanelets) { - const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); - findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); - } - for (const auto & poly : adjacent_lane_cv_polygons) { - cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); - } - - // (2) prepare unknown mask - // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] - // unknown: 255 - // not-unknown: 0 - cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); - cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); - for (int x = 0; x < width; x++) { - for (int y = 0; y < height; y++) { - const int idx = y * width + x; - const unsigned char intensity = occ_grid.data.at(idx); - if ( - planner_param_.occlusion.free_space_max <= intensity && - intensity < planner_param_.occlusion.occupied_min) { - unknown_mask_raw.at(height - 1 - y, x) = 255; - } - } - } - // (2.1) apply morphologyEx - const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); - cv::morphologyEx( - unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, - cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); - - // (3) occlusion mask - static constexpr unsigned char OCCLUDED = 255; - static constexpr unsigned char BLOCKED = 127; - cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); - cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); - // re-use attention_mask - attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); - } - std::vector> blocking_polygons; - for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); - findCommonCvPolygons(obj_poly.outer(), blocking_polygons); - } - for (const auto & blocking_polygon : blocking_polygons) { - cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); - } - for (const auto & division : lane_divisions) { - bool blocking_vehicle_found = false; - for (const auto & point_it : division) { - const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); - if (!valid) continue; - if (blocking_vehicle_found) { - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - continue; - } - if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { - blocking_vehicle_found = true; - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - } - } - } - - // (4) extract occlusion polygons - const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; - const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; - const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; - const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; - std::vector> contours; - cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); - std::vector> valid_contours; - for (const auto & contour : contours) { - if (contour.size() <= 2) { - continue; - } - std::vector approx_contour; - cv::approxPolyDP( - contour, approx_contour, - std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); - if (approx_contour.size() <= 2) continue; - // check area - const double poly_area = cv::contourArea(approx_contour); - if (poly_area < possible_object_area) continue; - // check bounding box size - const auto bbox = cv::minAreaRect(approx_contour); - if (const auto size = bbox.size; std::min(size.height, size.width) < - std::min(possible_object_bbox_x, possible_object_bbox_y) || - std::max(size.height, size.width) < - std::max(possible_object_bbox_x, possible_object_bbox_y)) { - continue; - } - valid_contours.push_back(approx_contour); - geometry_msgs::msg::Polygon polygon_msg; - geometry_msgs::msg::Point32 point_msg; - for (const auto & p : approx_contour) { - const double glob_x = (p.x + 0.5) * resolution + origin.x; - const double glob_y = (height - 0.5 - p.y) * resolution + origin.y; - point_msg.x = glob_x; - point_msg.y = glob_y; - point_msg.z = origin.z; - polygon_msg.points.push_back(point_msg); - } - debug_data_.occlusion_polygons.push_back(polygon_msg); - } - // (4.1) re-draw occluded cells using valid_contours - occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - for (const auto & valid_contour : valid_contours) { - // NOTE: drawContour does not work well - cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); - } - - // (5) find distance - // (5.1) discretize path_ip with resolution for computational cost - LineString2d path_linestring; - path_linestring.emplace_back( - path_ip.points.at(lane_start_idx).point.pose.position.x, - path_ip.points.at(lane_start_idx).point.pose.position.y); - { - auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; - for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { - const auto path_linestring_point = path_ip.points.at(i).point.pose.position; - if ( - tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < - 1.0 /* rough tick for computational cost */) { - continue; - } - path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); - prev_path_linestring_point = path_linestring_point; - } - } - - auto findNearestPointToProjection = []( - const lanelet::ConstLineString3d & division, - const Point2d & projection, const double dist_thresh) { - double min_dist = std::numeric_limits::infinity(); - auto nearest = division.end(); - for (auto it = division.begin(); it != division.end(); it++) { - const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); - if (dist < min_dist) { - min_dist = dist; - nearest = it; - } - if (dist < dist_thresh) { - break; - } - } - return nearest; - }; - struct NearestOcclusionPoint - { - int64 division_index{0}; - int64 point_index{0}; - double dist{0.0}; - geometry_msgs::msg::Point point; - geometry_msgs::msg::Point projection; - } nearest_occlusion_point; - double min_dist = std::numeric_limits::infinity(); - for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { - const auto & division = lane_divisions.at(division_index); - LineString2d division_linestring; - auto division_point_it = division.begin(); - division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); - for (auto point_it = division.begin(); point_it != division.end(); point_it++) { - if ( - std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < - 3.0 /* rough tick for computational cost */) { - continue; - } - division_linestring.emplace_back(point_it->x(), point_it->y()); - division_point_it = point_it; - } - - // find the intersection point of lane_line and path - std::vector intersection_points; - boost::geometry::intersection(division_linestring, path_linestring, intersection_points); - if (intersection_points.empty()) { - continue; - } - const auto & projection_point = intersection_points.at(0); - const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); - if (projection_it == division.end()) { - continue; - } - double acc_dist = 0.0; - auto acc_dist_it = projection_it; - for (auto point_it = projection_it; point_it != division.end(); point_it++) { - const double dist = - std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); - acc_dist += dist; - acc_dist_it = point_it; - const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - if (!valid) continue; - const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); - if (pixel == BLOCKED) { - break; - } - if (pixel == OCCLUDED) { - if (acc_dist < min_dist) { - min_dist = acc_dist; - nearest_occlusion_point = { - division_index, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; - } - } - } - } - - if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return OcclusionType::NOT_OCCLUDED; - } - - debug_data_.nearest_occlusion_projection = - std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - LineString2d ego_occlusion_line; - ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); - ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); - for (const auto & attention_object : target_objects.all_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - for (const auto & attention_object : target_objects.intersection_area_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - return OcclusionType::STATICALLY_OCCLUDED; -} - -static std::optional> -getFirstPointInsidePolygonsByFootprint( - const std::vector & polygons, - const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - - for (size_t i = start; i <= lane_end; ++i) { - const auto & pose = path_ip.points.at(i).point.pose; - const auto path_footprint = - tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (size_t j = 0; j < polygons.size(); ++j) { - const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); - const bool is_in_polygon = bg::intersects(area_2d, path_footprint); - if (is_in_polygon) { - return std::make_optional>(i, j); - } - } - } - return std::nullopt; -} - -void IntersectionLanelets::update( - const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, - lanelet::routing::RoutingGraphPtr routing_graph_ptr) -{ - is_prioritized_ = is_prioritized; - // find the first conflicting/detection area polygon intersecting the path - if (!first_conflicting_area_) { - auto first = getFirstPointInsidePolygonsByFootprint( - conflicting_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_conflicting_lane_ = conflicting_.at(first.value().second); - first_conflicting_area_ = conflicting_area_.at(first.value().second); - } - } - if (!first_attention_area_) { - const auto first = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_area_, interpolated_path_info, footprint, vehicle_length); - if (first) { - first_attention_lane_ = attention_non_preceding_.at(first.value().second); - first_attention_area_ = attention_non_preceding_area_.at(first.value().second); - } - } - if (first_attention_lane_ && !second_attention_lane_ && !second_attention_lane_empty_) { - const auto first_attention_lane = first_attention_lane_.value(); - // remove first_attention_area_ and non-straight lanelets from attention_non_preceding - lanelet::ConstLanelets attention_non_preceding_ex_first; - lanelet::ConstLanelets sibling_first_attention_lanelets; - for (const auto & previous : routing_graph_ptr->previous(first_attention_lane)) { - for (const auto & following : routing_graph_ptr->following(previous)) { - sibling_first_attention_lanelets.push_back(following); - } - } - for (const auto & ll : attention_non_preceding_) { - // the sibling lanelets of first_attention_lanelet are ruled out - if (lanelet::utils::contains(sibling_first_attention_lanelets, ll)) { - continue; - } - if (std::string(ll.attributeOr("turn_direction", "else")).compare("straight") == 0) { - attention_non_preceding_ex_first.push_back(ll); - } - } - if (attention_non_preceding_ex_first.empty()) { - second_attention_lane_empty_ = true; - } - const auto attention_non_preceding_ex_first_area = - getPolygon3dFromLanelets(attention_non_preceding_ex_first); - const auto second = getFirstPointInsidePolygonsByFootprint( - attention_non_preceding_ex_first_area, interpolated_path_info, footprint, vehicle_length); - if (second) { - second_attention_lane_ = attention_non_preceding_ex_first.at(second.value().second); - second_attention_area_ = second_attention_lane_.value().polygon3d(); - } + return intersection::Result>::make_ok( + intersection::Indecisive{"over the pass judge line. no plan needed"}); } + return intersection::Result>::make_err( + std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); } void TargetObject::calc_dist_to_stopline() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 1a9cf74624fc0..8a34aabe8b918 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -15,7 +15,11 @@ #ifndef SCENE_INTERSECTION_HPP_ #define SCENE_INTERSECTION_HPP_ -#include "util_type.hpp" +#include "decision_result.hpp" +#include "interpolated_path_info.hpp" +#include "intersection_lanelets.hpp" +#include "intersection_stoplines.hpp" +#include "result.hpp" #include #include @@ -26,271 +30,21 @@ #include #include -#include -#include +#include +#include +#include -#include -#include #include +#include #include #include +#include #include -#include #include namespace behavior_velocity_planner { -using TimeDistanceArray = std::vector>; - -struct DebugData -{ - std::optional collision_stop_wall_pose{std::nullopt}; - std::optional occlusion_stop_wall_pose{std::nullopt}; - std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional first_pass_judge_wall_pose{std::nullopt}; - bool passed_first_pass_judge{false}; - bool passed_second_pass_judge{false}; - std::optional second_pass_judge_wall_pose{std::nullopt}; - std::optional> attention_area{std::nullopt}; - std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; - std::optional> adjacent_area{std::nullopt}; - std::optional first_attention_area{std::nullopt}; - std::optional second_attention_area{std::nullopt}; - std::optional stuck_vehicle_detect_area{std::nullopt}; - std::optional> yield_stuck_detect_area{std::nullopt}; - std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - std::vector occlusion_polygons; - std::optional> - nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; - std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; -}; - -/** - * @struct - * @brief see the document for more details of IntersectionLanelets - */ -struct IntersectionLanelets -{ -public: - /** - * update conflicting lanelets and traffic priority information - */ - void update( - const bool is_prioritized, const util::InterpolatedPathInfo & interpolated_path_info, - const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length, - lanelet::routing::RoutingGraphPtr routing_graph_ptr); - - const lanelet::ConstLanelets & attention() const - { - return is_prioritized_ ? attention_non_preceding_ : attention_; - } - const std::vector> & attention_stoplines() const - { - return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; - } - const lanelet::ConstLanelets & conflicting() const { return conflicting_; } - const lanelet::ConstLanelets & adjacent() const { return adjacent_; } - const lanelet::ConstLanelets & occlusion_attention() const - { - return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; - } - const lanelet::ConstLanelets & attention_non_preceding() const - { - return attention_non_preceding_; - } - const std::vector & attention_area() const - { - return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; - } - const std::vector & conflicting_area() const - { - return conflicting_area_; - } - const std::vector & adjacent_area() const { return adjacent_area_; } - const std::vector & occlusion_attention_area() const - { - return occlusion_attention_area_; - } - const std::optional & first_conflicting_lane() const - { - return first_conflicting_lane_; - } - const std::optional & first_conflicting_area() const - { - return first_conflicting_area_; - } - const std::optional & first_attention_lane() const - { - return first_attention_lane_; - } - const std::optional & first_attention_area() const - { - return first_attention_area_; - } - const std::optional & second_attention_lane() const - { - return second_attention_lane_; - } - const std::optional & second_attention_area() const - { - return second_attention_area_; - } - - /** - * the set of attention lanelets which is topologically merged - */ - lanelet::ConstLanelets attention_; - std::vector attention_area_; - - /** - * the stop lines for each attention_ lanelets - */ - std::vector> attention_stoplines_; - - /** - * the conflicting part of attention lanelets - */ - lanelet::ConstLanelets attention_non_preceding_; - std::vector attention_non_preceding_area_; - - /** - * the stop lines for each attention_non_preceding_ - */ - std::vector> attention_non_preceding_stoplines_; - - /** - * the conflicting lanelets of the objective intersection lanelet - */ - lanelet::ConstLanelets conflicting_; - std::vector conflicting_area_; - - /** - * - */ - lanelet::ConstLanelets adjacent_; - std::vector adjacent_area_; - - /** - * the set of attention lanelets for occlusion detection which is topologically merged - */ - lanelet::ConstLanelets occlusion_attention_; - std::vector occlusion_attention_area_; - - /** - * the vector of sum of each occlusion_attention lanelet - */ - std::vector occlusion_attention_size_; - - /** - * the first conflicting lanelet which ego path points intersect for the first time - */ - std::optional first_conflicting_lane_{std::nullopt}; - std::optional first_conflicting_area_{std::nullopt}; - - /** - * the first attention lanelet which ego path points intersect for the first time - */ - std::optional first_attention_lane_{std::nullopt}; - std::optional first_attention_area_{std::nullopt}; - - /** - * the second attention lanelet which ego path points intersect next to the - * first_attention_lanelet - */ - bool second_attention_lane_empty_{false}; - std::optional second_attention_lane_{std::nullopt}; - std::optional second_attention_area_{std::nullopt}; - - /** - * flag if the intersection is prioritized by the traffic light - */ - bool is_prioritized_{false}; -}; - -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct IntersectionStopLines -{ - size_t closest_idx{0}; - - /** - * stuck_stopline is null if ego path does not intersect with first_conflicting_area - */ - std::optional stuck_stopline{std::nullopt}; - - /** - * default_stopline is null if it is calculated negative from first_attention_stopline - */ - std::optional default_stopline{std::nullopt}; - - /** - * first_attention_stopline is null if ego footprint along the path does not intersect with - * attention area. if path[0] satisfies the condition, it is 0 - */ - std::optional first_attention_stopline{std::nullopt}; - - /** - * second_attention_stopline is null if ego footprint along the path does not intersect with - * second_attention_lane. if path[0] satisfies the condition, it is 0 - */ - std::optional second_attention_stopline{std::nullopt}; - - /** - * occlusion_peeking_stopline is null if path[0] is already inside the attention area - */ - std::optional occlusion_peeking_stopline{std::nullopt}; - - /** - * first_pass_judge_line is before first_attention_stopline by the braking distance. if its value - * is calculated negative, it is 0 - */ - size_t first_pass_judge_line{0}; - - /** - * second_pass_judge_line is before second_attention_stopline by the braking distance. if - * second_attention_lane is null, it is same as first_pass_judge_line - */ - size_t second_pass_judge_line{0}; - - /** - * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with - * the centerline of the first_attention_lane - */ - size_t occlusion_wo_tl_pass_judge_line{0}; -}; - -/** - * @struct - * @brief see the document for more details of PathLanelets - */ -struct PathLanelets -{ - lanelet::ConstLanelets prev; - // lanelet::ConstLanelet entry2ego; this is included in `all` if exists - lanelet::ConstLanelet - ego_or_entry2exit; // this is `assigned lane` part of the path(not from - // ego) if ego is before the intersection, otherwise from ego to exit - std::optional next = - std::nullopt; // this is nullopt is the goal is inside intersection - lanelet::ConstLanelets all; - lanelet::ConstLanelets - conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the - // path -}; - /** * @struct * @brief see the document for more details of IntersectionStopLines @@ -317,19 +71,6 @@ struct TargetObjects std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy }; -/** - * @struct - * @brief categorize traffic light priority - */ -enum class TrafficPrioritizedLevel { - //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. - FULLY_PRIORITIZED = 0, - //! The target lane's traffic signal is amber - PARTIALLY_PRIORITIZED, - //! The target lane's traffic signal is green - NOT_PRIORITIZED -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -457,144 +198,77 @@ class IntersectionModule : public SceneModuleInterface RTC_OCCLUDED, }; - /** - * @struct - * @brief Internal error or ego already passed pass_judge_line - */ - struct Indecisive - { - std::string error; - }; - /** - * @struct - * @brief detected stuck vehicle - */ - struct StuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - std::optional occlusion_stopline_idx{std::nullopt}; - }; - /** - * @struct - * @brief yielded by vehicle on the attention area - */ - struct YieldStuckStop - { - size_t closest_idx{0}; - size_t stuck_stopline_idx{0}; - }; - /** - * @struct - * @brief only collision is detected - */ - struct NonOccludedCollisionStop - { - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - }; - /** - * @struct - * @brief occlusion is detected so ego needs to stop at the default stop line position - */ - struct FirstWaitBeforeOcclusion - { - bool is_actually_occlusion_cleared{false}; - size_t closest_idx{0}; - size_t first_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - }; - /** - * @struct - * @brief ego is approaching the boundary of attention area in the presence of traffic light - */ - struct PeekingTowardOcclusion + struct DebugData { - //! if intersection_occlusion is disapproved externally through RTC, it indicates - //! "is_forcefully_occluded" - bool is_actually_occlusion_cleared{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t first_attention_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it - //! contains the remaining time to release the static occlusion stuck and shows up - //! intersection_occlusion(x.y) - std::optional static_occlusion_timeout{std::nullopt}; - }; - /** - * @struct - * @brief both collision and occlusion are detected in the presence of traffic light - */ - struct OccludedCollisionStop - { - bool is_actually_occlusion_cleared{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t first_attention_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; - //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it - //! contains the remaining time to release the static occlusion stuck - std::optional static_occlusion_timeout{std::nullopt}; - }; - /** - * @struct - * @brief at least occlusion is detected in the absence of traffic light - */ - struct OccludedAbsenceTrafficLight - { - bool is_actually_occlusion_cleared{false}; - bool collision_detected{false}; - bool temporal_stop_before_attention_required{false}; - size_t closest_idx{0}; - size_t first_attention_area_stopline_idx{0}; - size_t peeking_limit_line_idx{0}; - }; - /** - * @struct - * @brief both collision and occlusion are not detected - */ - struct Safe - { - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional first_pass_judge_wall_pose{std::nullopt}; + bool passed_first_pass_judge{false}; + bool passed_second_pass_judge{false}; + std::optional second_pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional first_attention_area{std::nullopt}; + std::optional second_attention_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; + std::vector candidate_collision_object_polygons; + autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; + + using TimeDistanceArray = std::vector>; + /** * @struct - * @brief traffic light is red or arrow signal - */ - struct FullyPrioritized - { - bool collision_detected{false}; - size_t closest_idx{0}; - size_t collision_stopline_idx{0}; - size_t occlusion_stopline_idx{0}; + * @brief categorize traffic light priority + */ + enum class TrafficPrioritizedLevel { + //! The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + //! The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + //! The target lane's traffic signal is green + NOT_PRIORITIZED }; - using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line - StuckStop, //! detected stuck vehicle - YieldStuckStop, //! detected yield stuck vehicle - NonOccludedCollisionStop, //! detected collision while FOV is clear - FirstWaitBeforeOcclusion, //! stop for a while before peeking to occlusion - PeekingTowardOcclusion, //! peeking into occlusion while collision is not detected - OccludedCollisionStop, //! occlusion and collision are both detected - OccludedAbsenceTrafficLight, //! occlusion is detected in the absence of traffic light - Safe, //! judge as safe - FullyPrioritized //! only detect vehicles violating traffic rules - >; + /** @} */ IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const std::string & turn_direction, const bool has_traffic_light, - const bool enable_occlusion_detection, rclcpp::Node & node, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); - + const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup primary-function [fn] primary functions + * the entrypoint of this module is modifyPathVelocity() function that calculates safety decision + * of latest context and send it to RTC and then react to RTC approval. The reaction to RTC + * approval may not be based on the latest decision of this module depending on the auto-mode + * configuration. For module side it is not visible if the module is operating in auto-mode or + * manual-module. At first, initializeRTCStatus() is called to reset the safety value of + * INTERSECTION and INTERSECTION_OCCLUSION. Then modifyPathVelocityDetail() is called to analyze + * the context. Then prepareRTCStatus() is called to set the safety value of INTERSECTION and + * INTERSECTION_OCCLUSION. + * @{ + */ bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + /** @}*/ visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; @@ -609,137 +283,421 @@ class IntersectionModule : public SceneModuleInterface private: rclcpp::Node & node_; + + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup const-variables [var] const variables + * following variables are unique to this intersection lanelet or to this module + * @{ + */ + //! lanelet of this intersection const lanelet::Id lane_id_; + + //! associative(sibling) lanelets ids const std::set associative_ids_; + + //! turn_direction of this lane const std::string turn_direction_; + + //! flag if this intersection is traffic controlled const bool has_traffic_light_; - // Parameter + //! RTC uuid for INTERSECTION_OCCLUSION + const UUID occlusion_uuid_; + /** @}*/ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup semi-const-variables [var] semi-const variables + * following variables are immutable once initialized + * @{ + */ PlannerParam planner_param_; - std::optional intersection_lanelets_{std::nullopt}; + //! cache IntersectionLanelets struct + std::optional intersection_lanelets_{std::nullopt}; - // for pass judge decision - bool is_go_out_{false}; + //! cache discretized occlusion detection lanelets + std::optional> occlusion_attention_divisions_{ + std::nullopt}; + /** @}*/ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup pass-judge-variable [var] pass judge variables + * following variables are state variables that depends on how the vehicle passed the intersection + * @{ + */ + //! if true, this module never commands to STOP anymore bool is_permanent_go_{false}; - DecisionResult prev_decision_result_{Indecisive{""}}; - OcclusionType prev_occlusion_status_; + + //! for checking if ego is over the pass judge lines because previously the situation was SAFE + intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + + //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line + //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; + + //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is + //! expected after these variables are non-null, then it is the fault of past perception failure + //! at these time. std::optional safely_passed_1st_judge_line_time_{std::nullopt}; std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + /** @}*/ - // for occlusion detection - const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_{ - std::nullopt}; //! for caching discretized occlusion detection lanelets - StateMachine collision_state_machine_; //! for stable collision checking - StateMachine before_creep_state_machine_; //! for two phase stop - StateMachine occlusion_stop_state_machine_; //! for stable occlusion detection +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-variables [var] collision detection + * @{ + */ + //! debouncing for stable SAFE decision + StateMachine collision_state_machine_; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup occlusion-variables [var] occlusion detection variables + * @{ + */ + OcclusionType prev_occlusion_status_; + + //! debouncing for the first brief stop at the default stopline + StateMachine before_creep_state_machine_; + + //! debouncing for stable CLEARED decision + StateMachine occlusion_stop_state_machine_; + + //! debouncing for the brief stop at the boundary of attention area(if required by the flag) StateMachine temporal_stop_before_attention_state_machine_; + + //! time counter for the stuck detection due to occlusion caused static objects StateMachine static_occlusion_timeout_state_machine_; + /** @} */ std::optional initial_green_light_observed_time_{std::nullopt}; - // for RTC - const UUID occlusion_uuid_; +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup RTC-variables [var] RTC variables + * + * intersection module has additional rtc_interface_ for INTERSECTION_OCCLUSION in addition to the + * default rtc_interface of SceneModuleManagerInterfaceWithRTC. activated_ is the derived member + * of this module which is updated by the RTC config/service, so it should be read-only in this + * module. occlusion_safety_ and occlusion_stop_distance_ are the corresponding RTC value for + * INTERSECTION_OCCLUSION. + * @{ + */ bool occlusion_safety_{true}; double occlusion_stop_distance_{0.0}; bool occlusion_activated_{true}; bool occlusion_first_stop_required_{false}; + /** @}*/ +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @ingroup primary-functions + * @{ + */ /** - * @fn - * @brief set all RTC variable to safe and -inf + * @brief set all RTC variable to true(safe) and -INF */ void initializeRTCStatus(); + /** - * @fn * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); + intersection::DecisionResult modifyPathVelocityDetail( + PathWithLaneId * path, StopReason * stop_reason); + /** - * @fn * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const DecisionResult &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const intersection::DecisionResult &, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); /** - * @fn - * @brief find TrafficPrioritizedLevel + * @brief act based on current RTC approval */ - TrafficPrioritizedLevel getTrafficPrioritizedLevel(lanelet::ConstLanelet lane); + void reactRTCApproval( + const intersection::DecisionResult & decision_result, + autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + /** @}*/ +private: /** - * @fn - * @brief generate IntersectionLanelets + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup prepare-data [fn] basic data construction + * @{ */ - IntersectionLanelets getObjectiveLanelets( - lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path); + /** + * @struct + */ + struct BasicData + { + intersection::InterpolatedPathInfo interpolated_path_info; + intersection::IntersectionStopLines intersection_stoplines; + intersection::PathLanelets path_lanelets; + }; + + /** + * @brief prepare basic data structure + * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @note if successful, it is ensure that intersection_lanelets_, + * intersection_lanelets.first_conflicting_lane are not null + * + * To simplify modifyPathVelocityDetail(), this function is used at first + */ + intersection::Result prepareIntersectionData( + const bool is_prioritized, PathWithLaneId * path); + + /** + * @brief find the associated stopline road marking of assigned lanelet + */ + std::optional getStopLineIndexFromMap( + const intersection::InterpolatedPathInfo & interpolated_path_info, + lanelet::ConstLanelet assigned_lanelet); /** - * @fn * @brief generate IntersectionStopLines */ - std::optional generateIntersectionStopLines( + std::optional generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); /** - * @fn - * @brief find the associated stopline road marking of assigned lanelet + * @brief generate IntersectionLanelets */ - std::optional getStopLineIndexFromMap( - const util::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet); + intersection::IntersectionLanelets generateObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, + lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet); /** - * @fn * @brief generate PathLanelets */ - std::optional generatePathLanelets( + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, const std::vector & attention_areas, const size_t closest_idx); /** - * @fn - * @brief check stuck + * @brief generate discretized detection lane linestring. */ - bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets, DebugData * debug_data); + std::vector generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + /** @} */ +private: /** - * @fn - * @brief check yield stuck + * @defgroup utility [fn] utility member function + * @{ */ - bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const util::InterpolatedPathInfo & interpolated_path_info, - const lanelet::ConstLanelets & attention_lanelets, DebugData * debug_data); + void stoppedAtPositionForDuration( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, + const double duration, StateMachine * state_machine); + /** @} */ +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup get-traffic-light [fn] traffic light + * @{ + */ + /** + * @brief check if associated traffic light is green + */ + bool isGreenSolidOn() const; + + /** + * @brief find TrafficPrioritizedLevel + */ + TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; + /** @} */ + +private: /** - * @fn * @brief categorize target objects */ TargetObjects generateTargetObjects( - const IntersectionLanelets & intersection_lanelets, + const intersection::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup yield [fn] check stuck + * @{ + */ + /** + * @brief check stuck status + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() + */ + std::optional isStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::IntersectionStopLines & intersection_stoplines, + const intersection::PathLanelets & path_lanelets) const; + + bool isTargetStuckVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief check stuck + */ + bool checkStuckVehicleInIntersection(const intersection::PathLanelets & path_lanelets) const; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup yield [fn] check yield stuck + * @{ + */ + /** + * @brief check yield stuck status + * @attention this function has access to value() of intersection_lanelets_, + * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline + */ + std::optional isYieldStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects) const; + + /** + * @brief check yield stuck + */ + bool checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) const; + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup occlusion [fn] check occlusion + * @{ + */ + /** + * @brief check occlusion status + * @attention this function has access to value() of occlusion_attention_divisions_, + * intersection_lanelets.first_attention_area() + */ + std::tuple< + OcclusionType, bool /* module detection with margin */, + bool /* reconciled occlusion disapproval */> + getOcclusionStatus( + const TrafficPrioritizedLevel & traffic_prioritized_level, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionLanelets & intersection_lanelets, + const TargetObjects & target_objects); + + /** + * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + */ + OcclusionType detectOcclusion( + const std::vector & attention_areas, + const lanelet::ConstLanelets & adjacent_lanelets, + const lanelet::CompoundPolygon3d & first_attention_area, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const std::vector & lane_divisions, + const TargetObjects & target_objects); + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup pass-judge-decision [fn] pass judge decision + * @{ + */ + /** + * @brief check if ego is already over the pass judge line + * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return + * (is_over_1st_pass_judge, is_over_2nd_pass_judge) + * @attention this function has access to value() of intersection_stoplines.default_stopline, + * intersection_stoplines.occlusion_stopline + */ + intersection::Result< + intersection::Indecisive, + std::pair> + isOverPassJudgeLinesStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const intersection::IntersectionStopLines & intersection_stoplines); + /** @} */ + +private: + /** + *********************************************************** + *********************************************************** + *********************************************************** + * @defgroup collision-detection [fn] check collision + * @{ + */ + bool isTargetCollisionVehicleType( + const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + + /** + * @brief check if there are any objects around the stoplines on the attention areas when ego + * entered the intersection on green light + * @return return NonOccludedCollisionStop if there are vehicle within the margin for some + * duration from ego's entry to yield + * @attention this function has access to value() of + * intersection_stoplines.occlusion_peeking_stopline + */ + std::optional isGreenPseudoCollisionStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects); + /** - * @fn * @brief check collision */ bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const PathLanelets & path_lanelets, const size_t closest_idx, + const intersection::PathLanelets & path_lanelets, const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, const double time_delay, const TrafficPrioritizedLevel & traffic_prioritized_level); @@ -750,7 +708,6 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); /** - * @fn * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position */ @@ -758,28 +715,7 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, const double time_delay, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); - - std::vector generateDetectionLaneDivisions( - lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); - - /** - * @fn - * @brief check occlusion status - */ - OcclusionType getOcclusionStatus( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); - - /* - * @fn - * @brief check if associated traffic light is green - */ - bool isGreenSolidOn(lanelet::ConstLanelet lane); + /** @} */ /* bool IntersectionModule::checkFrontVehicleDeceleration( @@ -789,7 +725,7 @@ class IntersectionModule : public SceneModuleInterface const double assumed_front_car_decel); */ - DebugData debug_data_; + mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp new file mode 100644 index 0000000000000..43bb68cf56f67 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -0,0 +1,582 @@ +// Copyright 2024 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_intersection.hpp" +#include "util.hpp" + +#include // for toGeomPoly +#include // for smoothPath +#include +#include +#include // for toPolygon2d +#include + +#include +#include + +#include + +namespace +{ +tier4_autoware_utils::Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + namespace bg = boost::geometry; + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + tier4_autoware_utils::Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + tier4_autoware_utils::Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +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::TRAILER || + 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; +} + +std::optional +IntersectionModule::isGreenPseudoCollisionStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects) +{ + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = isGreenSolidOn(); + if (!is_green_solid_on) { + return std::nullopt; + } + const auto closest_idx = intersection_stoplines.closest_idx; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path.points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool still_wait = + (rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration); + if (!still_wait) { + return std::nullopt; + } + const bool exist_close_vehicles = std::any_of( + target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), + [&](const auto & object) { + return object.dist_to_stopline.has_value() && + object.dist_to_stopline.value() < + planner_param_.collision_detection.yield_on_green_traffic_light + .object_dist_to_stopline; + }); + if (exist_close_vehicles) { + return intersection::NonOccludedCollisionStop{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + } + } + return std::nullopt; +} + +bool IntersectionModule::checkCollision( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, + const intersection::PathLanelets & path_lanelets, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const TrafficPrioritizedLevel & traffic_prioritized_level) +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getPolygonFromArcLength; + + // check collision between target_objects predicted path and ego lane + // cut the predicted path at passing_time + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = calcIntersectionPassingTime( + path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + ego_ttc_time_array.stamp = path.header.stamp; + ego_ttc_pub_->publish(ego_ttc_time_array); + } + + const double passing_time = time_distance_array.back().first; + cutPredictPathWithDuration(target_objects, passing_time); + + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = getArcCoordinates( + concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // change TTC margin based on ego traffic light color + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { + if (!target_object.dist_to_stopline) { + return false; + } + const double dist_to_stopline = target_object.dist_to_stopline.value(); + if (dist_to_stopline < 0) { + return false; + } + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + return dist_to_stopline > braking_distance; + }; + const auto isTolerableOvershoot = [&](const TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stopline || + !target_object.stopline) { + return false; + } + const double dist_to_stopline = target_object.dist_to_stopline.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto stopline_front = target_object.stopline.value().front(); + const auto stopline_back = target_object.stopline.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; + const double stopline_to_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; + // check collision between predicted_path and ego_area + tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + object_ttc_time_array.layout.dim.resize(3); + object_ttc_time_array.layout.dim.at(0).label = "objects"; + object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id + object_ttc_time_array.layout.dim.at(1).label = + "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " + "start_time, start_dist, " + "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " + "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array.layout.dim.at(1).size = 57; + for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { + object_ttc_time_array.data.push_back(lane_id_); + } + bool collision_detected = false; + for (const auto & target_object : target_objects->all_attention_objects) { + const auto & object = target_object.object; + // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + expected_to_stop_before_stopline) { + debug_data_.amber_ignore_targets.objects.push_back(object); + continue; + } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } + for (const auto & predicted_path : object.kinematics.predicted_paths) { + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + // ignore the predicted path with too low confidence + continue; + } + + // collision point + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); + }); + if (first_itr == predicted_path.path.cend()) continue; + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); + }); + if (last_itr == predicted_path.path.crend()) continue; + + // possible collision time interval + 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 - collision_start_margin_time > 0) { + // start of possible ego position in the intersection + start_time_distance_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + ref_object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (start_time_distance_itr == time_distance_array.end()) { + // ego is already at the exit of intersection when npc is at collision point even if npc + // accelerates so ego's position interval is empty + 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 + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (end_time_distance_itr == time_distance_array.end()) { + // ego is already passing the intersection, when npc is is at collision point + // so ego's position interval is up to the end of intersection lane + 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 - + planner_data_->vehicle_info_.rear_overhang_m); + const double end_arc_length = std::min( + closest_arc_coords.length + (*end_time_distance_itr).second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + + const auto trimmed_ego_polygon = + getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length); + + if (trimmed_ego_polygon.empty()) { + continue; + } + + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + + for (auto itr = first_itr; itr != last_itr.base(); ++itr) { + const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); + if (bg::intersects(polygon, footprint_polygon)) { + collision_detected = true; + break; + } + } + object_ttc_time_array.layout.dim.at(0).size++; + const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & shape = object.shape; + object_ttc_time_array.data.insert( + object_ttc_time_array.data.end(), + {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), + shape.dimensions.x, shape.dimensions.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x, + 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, + start_time_distance_itr->first, start_time_distance_itr->second, + end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, + first_itr->position.y, last_itr->position.x, last_itr->position.y}); + for (unsigned i = 0; i < 20; i++) { + const auto & pos = + predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; + object_ttc_time_array.data.push_back(pos.x); + object_ttc_time_array.data.push_back(pos.y); + } + if (collision_detected) { + debug_data_.conflicting_targets.objects.push_back(object); + break; + } + } + } + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + object_ttc_time_array.stamp = path.header.stamp; + object_ttc_pub_->publish(object_ttc_time_array); + } + + return collision_detected; +} + +std::optional IntersectionModule::checkAngleForTargetLanelets( + + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const bool is_parked_vehicle) const +{ + const double detection_area_angle_thr = planner_param_.common.attention_area_angle_threshold; + const bool consider_wrong_direction_vehicle = + planner_param_.common.attention_area_angle_threshold; + const double dist_margin = planner_param_.common.attention_area_margin; + + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { + continue; + } + const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); + const double pose_angle = tf2::getYaw(pose.orientation); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); + if (consider_wrong_direction_vehicle) { + if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + } else { + if (std::fabs(angle_diff) < detection_area_angle_thr) { + return std::make_optional(i); + } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); + } + } + } + return std::nullopt; +} + +void IntersectionModule::cutPredictPathWithDuration( + TargetObjects * target_objects, const double time_thr) +{ + const rclcpp::Time current_time = clock_->now(); + for (auto & target_object : target_objects->all_attention_objects) { // each objects + for (auto & predicted_path : + target_object.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(target_objects->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); + } + } + } + } +} + +IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) +{ + const double intersection_velocity = + planner_param_.collision_detection.velocity_profile.default_velocity; + const double minimum_ego_velocity = + planner_param_.collision_detection.velocity_profile.minimum_default_velocity; + const bool use_upstream_velocity = + planner_param_.collision_detection.velocity_profile.use_upstream; + const double minimum_upstream_velocity = + planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + + int assigned_lane_found = false; + + // crop intersection part of the path, and set the reference velocity to intersection_velocity + // for ego's ttc + PathWithLaneId reference_path; + std::optional upstream_stopline{std::nullopt}; + for (size_t i = 0; i < path.points.size() - 1; ++i) { + auto reference_point = path.points.at(i); + // assume backward velocity is current ego velocity + if (i < closest_idx) { + reference_point.point.longitudinal_velocity_mps = current_velocity; + } + if ( + i > last_intersection_stopline_candidate_idx && + std::fabs(reference_point.point.longitudinal_velocity_mps) < + std::numeric_limits::epsilon() && + !upstream_stopline) { + upstream_stopline = i; + } + if (!use_upstream_velocity) { + reference_point.point.longitudinal_velocity_mps = intersection_velocity; + } + reference_path.points.push_back(reference_point); + bool has_objective_lane_id = util::hasLaneIds(path.points.at(i), associative_ids_); + 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. + } + + std::vector> original_path_xy; + for (size_t i = 0; i < reference_path.points.size(); ++i) { + const auto & p = reference_path.points.at(i).point.pose.position; + original_path_xy.emplace_back(p.x, p.y); + } + + // apply smoother to reference velocity + PathWithLaneId smoothed_reference_path = reference_path; + if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { + smoothed_reference_path = reference_path; + } + + // calculate when ego is going to reach each (interpolated) points on the path + TimeDistanceArray time_distance_array{}; + double dist_sum = 0.0; + double passing_time = time_delay; + time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stopline_candidate_idx` makes no sense + const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, path.points.at(closest_idx).point.pose, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + return motion_utils::findFirstNearestIndexWithSoftConstraints( + smoothed_reference_path.points, upstream_stopline_point, + planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); + } else { + return std::nullopt; + } + }(); + + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + const auto & p1 = smoothed_reference_path.points.at(i); + const auto & p2 = smoothed_reference_path.points.at(i + 1); + + const double dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + // use average velocity between p1 and p2 + const double average_velocity = + (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; + const double passing_velocity = [=]() { + if (use_upstream_velocity) { + if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { + return minimum_upstream_velocity; + } + return std::max(average_velocity, minimum_ego_velocity); + } else { + return std::max(average_velocity, minimum_ego_velocity); + } + }(); + passing_time += (dist / passing_velocity); + + time_distance_array.emplace_back(passing_time, dist_sum); + } + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + debug_ttc_array->data.reserve( + time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id_); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.x); + } + for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { + const auto & p = smoothed_reference_path.points.at(i).point.pose.position; + debug_ttc_array->data.push_back(p.y); + } + return time_distance_array; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp new file mode 100644 index 0000000000000..353826070eff7 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -0,0 +1,407 @@ +// Copyright 2024 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_intersection.hpp" +#include "util.hpp" + +#include +#include // for toPolygon2d + +#include +#include + +#include + +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +std::tuple< + IntersectionModule::OcclusionType, bool /* module detection with margin */, + bool /* reconciled occlusion disapproval */> +IntersectionModule::getOcclusionStatus( + const TrafficPrioritizedLevel & traffic_prioritized_level, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionLanelets & intersection_lanelets, + const TargetObjects & target_objects) +{ + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); + const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + + const bool is_amber_or_red = + (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); + // check occlusion on detection lane + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); + auto occlusion_status = + (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) + ? detectOcclusion( + occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, + occlusion_attention_divisions, target_objects) + : OcclusionType::NOT_OCCLUDED; + occlusion_stop_state_machine_.setStateWithMarginTime( + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, + logger_.get_child("occlusion_stop"), *clock_); + const bool is_occlusion_cleared_with_margin = + (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection + // distinguish if ego detected occlusion or RTC detects occlusion + const bool ext_occlusion_requested = + (is_occlusion_cleared_with_margin && !occlusion_activated_); // RTC's detection + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } + const bool is_occlusion_state = + (!is_occlusion_cleared_with_margin || ext_occlusion_requested); // including approval + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } + return {occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state}; +} + +IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( + const std::vector & attention_areas, + const lanelet::ConstLanelets & adjacent_lanelets, + const lanelet::CompoundPolygon3d & first_attention_area, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const std::vector & lane_divisions, + const TargetObjects & target_objects) +{ + const auto & occ_grid = *planner_data_->occupancy_grid; + const auto & current_pose = planner_data_->current_odometry->pose; + const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; + + const auto & path_ip = interpolated_path_info.path; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + + const auto first_attention_area_idx = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); + if (!first_attention_area_idx) { + return OcclusionType::NOT_OCCLUDED; + } + + const auto first_inside_attention_idx_ip_opt = + util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); + const std::pair lane_attention_interval_ip = + first_inside_attention_idx_ip_opt + ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) + : lane_interval_ip; + const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; + + const int width = occ_grid.info.width; + const int height = occ_grid.info.height; + const double resolution = occ_grid.info.resolution; + const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; + + Polygon2d grid_poly; + grid_poly.outer().emplace_back(origin.x, origin.y); + grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); + grid_poly.outer().emplace_back( + origin.x + (width - 1) * resolution, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y + (height - 1) * resolution); + grid_poly.outer().emplace_back(origin.x, origin.y); + bg::correct(grid_poly); + + auto findCommonCvPolygons = + [&](const auto & area2d, std::vector> & cv_polygons) -> void { + tier4_autoware_utils::Polygon2d area2d_poly; + for (const auto & p : area2d) { + area2d_poly.outer().emplace_back(p.x(), p.y()); + } + area2d_poly.outer().push_back(area2d_poly.outer().front()); + bg::correct(area2d_poly); + std::vector common_areas; + bg::intersection(area2d_poly, grid_poly, common_areas); + if (common_areas.empty()) { + return; + } + for (size_t i = 0; i < common_areas.size(); ++i) { + common_areas[i].outer().push_back(common_areas[i].outer().front()); + bg::correct(common_areas[i]); + } + for (const auto & common_area : common_areas) { + std::vector cv_polygon; + for (const auto & p : common_area.outer()) { + const int idx_x = static_cast((p.x() - origin.x) / resolution); + const int idx_y = static_cast((p.y() - origin.y) / resolution); + cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + } + cv_polygons.push_back(cv_polygon); + } + }; + + // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 + // NOTE: interesting area is set to 255 for later masking + cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); + std::vector> attention_area_cv_polygons; + for (const auto & attention_area : attention_areas) { + const auto area2d = lanelet::utils::to2D(attention_area); + findCommonCvPolygons(area2d, attention_area_cv_polygons); + } + for (const auto & poly : attention_area_cv_polygons) { + cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); + } + // (1.1) + // reset adjacent_lanelets area to 0 on attention_mask + std::vector> adjacent_lane_cv_polygons; + for (const auto & adjacent_lanelet : adjacent_lanelets) { + const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); + findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); + } + for (const auto & poly : adjacent_lane_cv_polygons) { + cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); + } + + // (2) prepare unknown mask + // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] + // unknown: 255 + // not-unknown: 0 + cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); + for (int x = 0; x < width; x++) { + for (int y = 0; y < height; y++) { + const int idx = y * width + x; + const unsigned char intensity = occ_grid.data.at(idx); + if ( + planner_param_.occlusion.free_space_max <= intensity && + intensity < planner_param_.occlusion.occupied_min) { + unknown_mask_raw.at(height - 1 - y, x) = 255; + } + } + } + // (2.1) apply morphologyEx + const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); + cv::morphologyEx( + unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); + + // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; + cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // re-use attention_mask + attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + const auto & blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + } + std::vector> blocking_polygons; + for (const auto & blocking_attention_object : blocking_attention_objects) { + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); + } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + } + } + } + + // (4) extract occlusion polygons + const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; + const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; + const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; + const double possible_object_area = possible_object_bbox_x * possible_object_bbox_y; + std::vector> contours; + cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + std::vector> valid_contours; + for (const auto & contour : contours) { + if (contour.size() <= 2) { + continue; + } + std::vector approx_contour; + cv::approxPolyDP( + contour, approx_contour, + std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); + if (approx_contour.size() <= 2) continue; + // check area + const double poly_area = cv::contourArea(approx_contour); + if (poly_area < possible_object_area) continue; + // check bounding box size + const auto bbox = cv::minAreaRect(approx_contour); + if (const auto size = bbox.size; std::min(size.height, size.width) < + std::min(possible_object_bbox_x, possible_object_bbox_y) || + std::max(size.height, size.width) < + std::max(possible_object_bbox_x, possible_object_bbox_y)) { + continue; + } + valid_contours.push_back(approx_contour); + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : approx_contour) { + const double glob_x = (p.x + 0.5) * resolution + origin.x; + const double glob_y = (height - 0.5 - p.y) * resolution + origin.y; + point_msg.x = glob_x; + point_msg.y = glob_y; + point_msg.z = origin.z; + polygon_msg.points.push_back(point_msg); + } + debug_data_.occlusion_polygons.push_back(polygon_msg); + } + // (4.1) re-draw occluded cells using valid_contours + occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + for (const auto & valid_contour : valid_contours) { + // NOTE: drawContour does not work well + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); + } + + // (5) find distance + // (5.1) discretize path_ip with resolution for computational cost + LineString2d path_linestring; + path_linestring.emplace_back( + path_ip.points.at(lane_start_idx).point.pose.position.x, + path_ip.points.at(lane_start_idx).point.pose.position.y); + { + auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; + for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { + const auto path_linestring_point = path_ip.points.at(i).point.pose.position; + if ( + tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + 1.0 /* rough tick for computational cost */) { + continue; + } + path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); + prev_path_linestring_point = path_linestring_point; + } + } + + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; + } + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; + struct NearestOcclusionPoint + { + int64 division_index{0}; + int64 point_index{0}; + double dist{0.0}; + geometry_msgs::msg::Point point; + geometry_msgs::msg::Point projection; + } nearest_occlusion_point; + double min_dist = std::numeric_limits::infinity(); + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { + continue; + } + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + division_index, std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + } + } + } + } + + if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { + return OcclusionType::NOT_OCCLUDED; + } + + debug_data_.nearest_occlusion_projection = + std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); + LineString2d ego_occlusion_line; + ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + for (const auto & attention_object : target_objects.all_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + for (const auto & attention_object : target_objects.intersection_area_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + return OcclusionType::STATICALLY_OCCLUDED; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp new file mode 100644 index 0000000000000..746e615d8c6b0 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -0,0 +1,869 @@ +// Copyright 2024 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_intersection.hpp" +#include "util.hpp" + +#include // for to_bg2d +#include // for planning_utils:: +#include +#include // for lanelet::autoware::RoadMarking +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + +namespace +{ +namespace bg = boost::geometry; + +lanelet::ConstLanelets getPrevLanelets( + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) +{ + lanelet::ConstLanelets previous_lanelets; + for (const auto & ll : lanelets_on_path) { + if (associative_ids.find(ll.id()) != associative_ids.end()) { + return previous_lanelets; + } + previous_lanelets.push_back(ll); + } + return previous_lanelets; +} + +// end inclusive +lanelet::ConstLanelet generatePathLanelet( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const size_t end_idx, const double width, const double interval) +{ + lanelet::Points3d lefts; + lanelet::Points3d rights; + size_t prev_idx = start_idx; + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto & p_prev = path.points.at(prev_idx).point.pose; + if (i != start_idx && tier4_autoware_utils::calcDistance2d(p_prev, p) < interval) { + continue; + } + prev_idx = i; + const double yaw = tf2::getYaw(p.orientation); + const double x = p.position.x; + const double y = p.position.y; + // NOTE: maybe this is opposite + const double left_x = x + width / 2 * std::sin(yaw); + const double left_y = y - width / 2 * std::cos(yaw); + const double right_x = x - width / 2 * std::sin(yaw); + const double right_y = y + width / 2 * std::cos(yaw); + lefts.emplace_back(lanelet::InvalId, left_x, left_y, p.position.z); + rights.emplace_back(lanelet::InvalId, right_x, right_y, p.position.z); + } + lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, lefts); + lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, rights); + + return lanelet::Lanelet(lanelet::InvalId, left, right); +} + +std::optional> getFirstPointInsidePolygons( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const std::pair lane_interval, + const std::vector & polygons, const bool search_forward = true) +{ + if (search_forward) { + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + bool is_in_lanelet = false; + const 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(behavior_velocity_planner::to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + } + } else { + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + bool is_in_lanelet = false; + const 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(behavior_velocity_planner::to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional>( + i, polygon); + } + } + if (is_in_lanelet) { + break; + } + if (i == 0) { + break; + } + } + } + return std::nullopt; +} + +double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +intersection::Result +IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) +{ + using intersection::Result; + + const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const auto & current_pose = planner_data_->current_odometry->pose; + + // spline interpolation + const auto interpolated_path_info_opt = util::generateInterpolatedPath( + lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); + if (!interpolated_path_info_opt) { + return Result::make_err( + intersection::Indecisive{"splineInterpolate failed"}); + } + + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + if (!interpolated_path_info.lane_id_interval) { + return Result::make_err( + intersection::Indecisive{ + "Path has no interval on intersection lane " + std::to_string(lane_id_)}); + } + + // cache intersection lane information because it is invariant + if (!intersection_lanelets_) { + intersection_lanelets_ = + generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + } + auto & intersection_lanelets = intersection_lanelets_.value(); + + // at the very first time of regisTration of this module, the path may not be conflicting with the + // attention area, so update() is called to update the internal data as well as traffic light info + intersection_lanelets.update( + is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); + + const auto & conflicting_lanelets = intersection_lanelets.conflicting(); + const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); + const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { + // this is abnormal + return Result::make_err( + intersection::Indecisive{"conflicting area is empty"}); + } + const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); + const auto & first_conflicting_area = first_conflicting_area_opt.value(); + const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); + + // generate all stop line candidates + // see the doc for struct IntersectionStopLines + /// even if the attention area is null, stuck vehicle stop line needs to be generated from + /// conflicting lanes + const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() + ? intersection_lanelets.first_attention_lane().value() + : first_conflicting_lane; + + const auto intersection_stoplines_opt = generateIntersectionStopLines( + assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, + interpolated_path_info, path); + if (!intersection_stoplines_opt) { + return Result::make_err( + intersection::Indecisive{"failed to generate intersection_stoplines"}); + } + const auto & intersection_stoplines = intersection_stoplines_opt.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); + const auto & conflicting_area = intersection_lanelets.conflicting_area(); + const auto lanelets_on_path = + planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); + // see the doc for struct PathLanelets + const auto path_lanelets_opt = generatePathLanelets( + lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, + first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); + if (!path_lanelets_opt.has_value()) { + return Result::make_err( + intersection::Indecisive{"failed to generate PathLanelets"}); + } + const auto & path_lanelets = path_lanelets_opt.value(); + + if (!occlusion_attention_divisions_) { + occlusion_attention_divisions_ = generateDetectionLaneDivisions( + intersection_lanelets.occlusion_attention(), routing_graph_ptr, + planner_data_->occupancy_grid->info.resolution); + } + + return Result::make_ok( + BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); +} + +std::optional IntersectionModule::getStopLineIndexFromMap( + const intersection::InterpolatedPathInfo & interpolated_path_info, + lanelet::ConstLanelet assigned_lanelet) +{ + const auto & path = interpolated_path_info.path; + const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); + + const auto road_markings = + assigned_lanelet.regulatoryElementsAs(); + lanelet::ConstLineStrings3d stopline; + for (const auto & road_marking : road_markings) { + const std::string type = + road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); + if (type == lanelet::AttributeValueString::StopLine) { + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. + } + } + if (stopline.empty()) { + return std::nullopt; + } + + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = + planning_utils::extendLine(p_start, p_end, planner_data_->stop_line_extend_length); + + for (size_t i = lane_interval.first; i < lane_interval.second; 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_stopline, path_segment, collision_points); + + if (collision_points.empty()) { + continue; + } + + return i; + } + + geometry_msgs::msg::Pose stop_point_from_map; + stop_point_from_map.position.x = 0.5 * (p_start.x() + p_end.x()); + stop_point_from_map.position.y = 0.5 * (p_start.y() + p_end.y()); + stop_point_from_map.position.z = 0.5 * (p_start.z() + p_end.z()); + + return motion_utils::findFirstNearestIndexWithSoftConstraints( + path.points, stop_point_from_map, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); +} + +std::optional +IntersectionModule::generateIntersectionStopLines( + lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, + const lanelet::ConstLanelet & first_attention_lane, + const std::optional & second_attention_area_opt, + const intersection::InterpolatedPathInfo & interpolated_path_info, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) +{ + const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; + const double stopline_margin = planner_param_.common.default_stopline_margin; + const double max_accel = planner_param_.common.max_accel; + const double max_jerk = planner_param_.common.max_jerk; + const double delay_response_time = planner_param_.common.delay_response_time; + const double peeking_offset = planner_param_.occlusion.peeking_offset; + + const auto first_attention_area = first_attention_lane.polygon3d(); + const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); + const auto & path_ip = interpolated_path_info.path; + const double ds = interpolated_path_info.ds; + const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); + const int base2front_idx_dist = std::ceil(baselink2front / ds); + + // find the index of the first point whose vehicle footprint on it intersects with attention_area + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + const std::optional first_footprint_inside_1st_attention_ip_opt = + util::getFirstPointInsidePolygonByFootprint( + first_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (!first_footprint_inside_1st_attention_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_1st_attention_ip = + first_footprint_inside_1st_attention_ip_opt.value(); + + std::optional first_footprint_attention_centerline_ip_opt = std::nullopt; + for (auto i = std::get<0>(lane_interval_ip); i < std::get<1>(lane_interval_ip); ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, first_attention_lane_centerline.basicLineString())) { + // NOTE: maybe consideration of braking dist is necessary + first_footprint_attention_centerline_ip_opt = i; + break; + } + } + if (!first_footprint_attention_centerline_ip_opt) { + return std::nullopt; + } + const size_t first_footprint_attention_centerline_ip = + first_footprint_attention_centerline_ip_opt.value(); + + // (1) default stop line position on interpolated path + bool default_stopline_valid = true; + int stop_idx_ip_int = -1; + if (const auto map_stop_idx_ip = + getStopLineIndexFromMap(interpolated_path_info, assigned_lanelet); + map_stop_idx_ip) { + stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; + } + if (stop_idx_ip_int < 0) { + stop_idx_ip_int = first_footprint_inside_1st_attention_ip - stopline_margin_idx_dist; + } + if (stop_idx_ip_int < 0) { + default_stopline_valid = false; + } + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + + // (2) ego front stop line position on interpolated path + const geometry_msgs::msg::Pose & current_pose = planner_data_->current_odometry->pose; + const auto closest_idx_ip = motion_utils::findFirstNearestIndexWithSoftConstraints( + path_ip.points, current_pose, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + + // (3) occlusion peeking stop line position on interpolated path + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); + bool occlusion_peeking_line_valid = true; + // NOTE: if footprints[0] is already inside the attention area, invalid + { + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_attention_area).basicPolygon())) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + occlusion_peeking_line_ip_int = + first_footprint_inside_1st_attention_ip + std::ceil(peeking_offset / ds); + } + const auto occlusion_peeking_line_ip = static_cast( + std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + + // (4) first attention stopline position on interpolated path + const auto first_attention_stopline_ip = first_footprint_inside_1st_attention_ip; + const bool first_attention_stopline_valid = true; + + // (5) 1st pass judge line position on interpolated path + const double velocity = planner_data_->current_velocity->twist.linear.x; + const double acceleration = planner_data_->current_acceleration->accel.accel.linear.x; + const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( + velocity, acceleration, max_accel, max_jerk, delay_response_time); + int first_pass_judge_ip_int = + static_cast(first_footprint_inside_1st_attention_ip) - std::ceil(braking_dist / ds); + const auto first_pass_judge_line_ip = static_cast( + std::clamp(first_pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); + const auto occlusion_wo_tl_pass_judge_line_ip = static_cast(std::max( + 0, static_cast(first_footprint_attention_centerline_ip) - std::ceil(braking_dist / ds))); + + // (6) stuck vehicle stopline position on interpolated path + int stuck_stopline_ip_int = 0; + bool stuck_stopline_valid = true; + if (use_stuck_stopline) { + // NOTE: when ego vehicle is approaching attention area and already passed + // first_conflicting_area, this could be null. + const auto stuck_stopline_idx_ip_opt = util::getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); + if (!stuck_stopline_idx_ip_opt) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; + } else { + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; + } + } else { + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); + } + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; + } + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); + + // (7) second attention stopline position on interpolated path + int second_attention_stopline_ip_int = -1; + bool second_attention_stopline_valid = false; + if (second_attention_area_opt) { + const auto & second_attention_area = second_attention_area_opt.value(); + std::optional first_footprint_inside_2nd_attention_ip_opt = + util::getFirstPointInsidePolygonByFootprint( + second_attention_area, interpolated_path_info, local_footprint, baselink2front); + if (first_footprint_inside_2nd_attention_ip_opt) { + second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + second_attention_stopline_valid = true; + } + } + const auto second_attention_stopline_ip = + second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) + : 0; + + // (8) second pass judge line position on interpolated path. It is the same as first pass judge + // line if second_attention_lane is null + int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; + const auto second_pass_judge_line_ip = + second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) + : first_pass_judge_line_ip; + + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t second_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; + size_t first_pass_judge_line{0}; + size_t second_pass_judge_line{0}; + size_t occlusion_wo_tl_pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&second_attention_stopline_ip, &intersection_stoplines_temp.second_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&first_pass_judge_line_ip, &intersection_stoplines_temp.first_pass_judge_line}, + {&second_pass_judge_line_ip, &intersection_stoplines_temp.second_pass_judge_line}, + {&occlusion_wo_tl_pass_judge_line_ip, + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( + [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { + const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; + const auto insert_idx = util::insertPointIndex( + insert_point, original_path, planner_data_->ego_nearest_dist_threshold, + planner_data_->ego_nearest_yaw_threshold); + if (!insert_idx) { + return std::nullopt; + } + *stop_idx = insert_idx.value(); + } + if ( + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; + } + + intersection::IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; + } + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; + } + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; + } + if (second_attention_stopline_valid) { + intersection_stoplines.second_attention_stopline = + intersection_stoplines_temp.second_attention_stopline; + } + if (occlusion_peeking_line_valid) { + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; + } + intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + intersection_stoplines.occlusion_wo_tl_pass_judge_line = + intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; + return intersection_stoplines; +} + +intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( + lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, + const lanelet::ConstLanelet assigned_lanelet) +{ + const double detection_area_length = planner_param_.common.attention_area_length; + const double occlusion_detection_area_length = + planner_param_.occlusion.occlusion_attention_area_length; + const bool consider_wrong_direction_vehicle = + planner_param_.collision_detection.consider_wrong_direction_vehicle; + + // retrieve a stopline associated with a traffic light + bool has_traffic_light = false; + if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + + // for low priority lane + // If ego_lane has right of way (i.e. is high priority), + // ignore yieldLanelets (i.e. low priority lanes) + lanelet::ConstLanelets yield_lanelets{}; + 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); + } + } + } + } + + // get all following lanes of previous lane + lanelet::ConstLanelets ego_lanelets{}; + 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 adjacent_followings; + + for (const auto & conflicting_lanelet : conflicting_lanelets) { + for (const auto & following_lanelet : routing_graph_ptr->following(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + for (const auto & following_lanelet : routing_graph_ptr->previous(conflicting_lanelet)) { + adjacent_followings.push_back(following_lanelet); + } + } + + // final objective lanelets + lanelet::ConstLanelets detection_lanelets; + lanelet::ConstLanelets conflicting_ex_ego_lanelets; + // conflicting lanes is necessary to get stopline for stuck vehicle + for (auto && conflicting_lanelet : conflicting_lanelets) { + if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) + conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); + } + + // exclude yield lanelets and ego lanelets from detection_lanelets + if (turn_direction_ == std::string("straight") && has_traffic_light) { + // if assigned lanelet is "straight" with traffic light, detection area is not necessary + } else { + if (consider_wrong_direction_vehicle) { + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if (lanelet::utils::contains(yield_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + for (const auto & adjacent_following : adjacent_followings) { + detection_lanelets.push_back(adjacent_following); + } + } else { + // otherwise we need to know the priority from RightOfWay + for (const auto & conflicting_lanelet : conflicting_lanelets) { + if ( + lanelet::utils::contains(yield_lanelets, conflicting_lanelet) || + lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) { + continue; + } + detection_lanelets.push_back(conflicting_lanelet); + } + } + } + + // get possible lanelet path that reaches conflicting_lane longer than given length + lanelet::ConstLanelets detection_and_preceding_lanelets; + { + const double length = detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) detection_and_preceding_lanelets.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, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) detection_and_preceding_lanelets.push_back(l); + } + } + } + } + + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets; + { + const double length = occlusion_detection_area_length; + std::set detection_ids; + for (const auto & ll : detection_lanelets) { + // Preceding lanes does not include detection_lane so add them at the end + const auto & inserted = detection_ids.insert(ll.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.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, length, ego_lanelets); + for (const auto & ls : lanelet_sequences) { + for (const auto & l : ls) { + const auto & inserted = detection_ids.insert(l.id()); + if (inserted.second) occlusion_detection_and_preceding_lanelets.push_back(l); + } + } + } + } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const std::string turn_direction = ll.attributeOr("turn_direction", "else"); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); + + intersection::IntersectionLanelets result; + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stopline{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + break; + } + if (stopline) break; + } + result.attention_stoplines_.push_back(stopline); + } + result.attention_non_preceding_ = std::move(detection_lanelets); + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + std::optional stopline = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); + } + result.attention_non_preceding_stoplines_.push_back(stopline); + } + result.conflicting_ = std::move(conflicting_ex_ego_lanelets); + result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids_); + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched + result.attention_area_ = util::getPolygon3dFromLanelets(result.attention_); + result.attention_non_preceding_area_ = + util::getPolygon3dFromLanelets(result.attention_non_preceding_); + result.conflicting_area_ = util::getPolygon3dFromLanelets(result.conflicting_); + result.adjacent_area_ = util::getPolygon3dFromLanelets(result.adjacent_); + result.occlusion_attention_area_ = util::getPolygon3dFromLanelets(result.occlusion_attention_); + return result; +} + +std::optional IntersectionModule::generatePathLanelets( + const lanelet::ConstLanelets & lanelets_on_path, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & first_conflicting_area, + const std::vector & conflicting_areas, + const std::optional & first_attention_area, + const std::vector & attention_areas, const size_t closest_idx) +{ + const double width = planner_data_->vehicle_info_.vehicle_width_m; + static constexpr double path_lanelet_interval = 1.5; + + const auto & assigned_lane_interval_opt = interpolated_path_info.lane_id_interval; + if (!assigned_lane_interval_opt) { + return std::nullopt; + } + const auto assigned_lane_interval = assigned_lane_interval_opt.value(); + const auto & path = interpolated_path_info.path; + + intersection::PathLanelets path_lanelets; + // prev + path_lanelets.prev = ::getPrevLanelets(lanelets_on_path, associative_ids_); + path_lanelets.all = path_lanelets.prev; + + // entry2ego if exist + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; + if (closest_idx > assigned_lane_start) { + path_lanelets.all.push_back( + ::generatePathLanelet(path, assigned_lane_start, closest_idx, width, path_lanelet_interval)); + } + + // ego_or_entry2exit + const auto ego_or_entry_start = std::max(closest_idx, assigned_lane_start); + path_lanelets.ego_or_entry2exit = + generatePathLanelet(path, ego_or_entry_start, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.ego_or_entry2exit); + + // next + if (assigned_lane_end < path.points.size() - 1) { + const int next_id = path.points.at(assigned_lane_end).lane_ids.at(0); + const auto next_lane_interval_opt = util::findLaneIdsInterval(path, {next_id}); + if (next_lane_interval_opt) { + const auto [next_start, next_end] = next_lane_interval_opt.value(); + path_lanelets.next = + generatePathLanelet(path, next_start, next_end, width, path_lanelet_interval); + path_lanelets.all.push_back(path_lanelets.next.value()); + } + } + + const auto first_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_attention_area.value()) + : util::getFirstPointInsidePolygon(path, assigned_lane_interval, first_conflicting_area); + const auto last_inside_conflicting_idx_opt = + first_attention_area.has_value() + ? ::getFirstPointInsidePolygons(path, assigned_lane_interval, attention_areas, false) + : ::getFirstPointInsidePolygons(path, assigned_lane_interval, conflicting_areas, false); + if (first_inside_conflicting_idx_opt && last_inside_conflicting_idx_opt) { + const auto first_inside_conflicting_idx = first_inside_conflicting_idx_opt.value(); + const auto last_inside_conflicting_idx = last_inside_conflicting_idx_opt.value().first; + lanelet::ConstLanelet conflicting_interval = generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width, + path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width, path_lanelet_interval); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } + return path_lanelets; +} + +std::vector IntersectionModule::generateDetectionLaneDivisions( + lanelet::ConstLanelets detection_lanelets_all, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) +{ + const double curvature_threshold = + planner_param_.occlusion.attention_lane_crop_curvature_threshold; + const double curvature_calculation_ds = + planner_param_.occlusion.attention_lane_curvature_calculation_ds; + + using lanelet::utils::getCenterlineWithOffset; + + // (0) remove left/right lanelet + lanelet::ConstLanelets detection_lanelets; + for (const auto & detection_lanelet : detection_lanelets_all) { + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = ::getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } + detection_lanelets.push_back(detection_lanelet); + } + + // (1) tsort detection_lanelets + const auto [merged_detection_lanelets, originals] = + util::mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); + + // (2) merge each branch to one lanelet + // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); + double area = 0; + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); + } + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); + } + + // (3) discretize each merged lanelet + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); + const double width = area / length; + for (int i = 0; i < static_cast(width / resolution); ++i) { + const double offset = resolution * i - width / 2; + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); + } + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); + } + return detection_divisions; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp new file mode 100644 index 0000000000000..7f23bed3c36cd --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -0,0 +1,380 @@ +// Copyright 2024 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_intersection.hpp" +#include "util.hpp" + +#include // for toGeomPoly +#include +#include + +#include +#include +#include + +#include +#include + +namespace +{ +lanelet::LineString3d getLineStringFromArcLength( + const lanelet::ConstLineString3d & linestring, const double s1, const double s2) +{ + lanelet::Points3d points; + double accumulated_length = 0; + size_t start_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s1) { + start_index = i; + break; + } + accumulated_length += length; + } + if (start_index < linestring.size() - 1) { + const auto & p1 = linestring[start_index]; + const auto & p2 = linestring[start_index + 1]; + const double residue = s1 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto start_basic_point = p1.basicPoint() + residue * direction_vector; + const auto start_point = lanelet::Point3d(lanelet::InvalId, start_basic_point); + points.push_back(start_point); + } + + accumulated_length = 0; + size_t end_index = linestring.size(); + for (size_t i = 0; i < linestring.size() - 1; i++) { + const auto & p1 = linestring[i]; + const auto & p2 = linestring[i + 1]; + const double length = boost::geometry::distance(p1.basicPoint(), p2.basicPoint()); + if (accumulated_length + length > s2) { + end_index = i; + break; + } + accumulated_length += length; + } + + for (size_t i = start_index + 1; i < end_index; i++) { + const auto p = lanelet::Point3d(linestring[i]); + points.push_back(p); + } + if (end_index < linestring.size() - 1) { + const auto & p1 = linestring[end_index]; + const auto & p2 = linestring[end_index + 1]; + const double residue = s2 - accumulated_length; + const auto direction_vector = (p2.basicPoint() - p1.basicPoint()).normalized(); + const auto end_basic_point = p1.basicPoint() + residue * direction_vector; + const auto end_point = lanelet::Point3d(lanelet::InvalId, end_basic_point); + points.push_back(end_point); + } + return lanelet::LineString3d{lanelet::InvalId, points}; +} + +lanelet::ConstLanelet createLaneletFromArcLength( + const lanelet::ConstLanelet & lanelet, const double s1, const double s2) +{ + const double total_length = boost::geometry::length(lanelet.centerline2d().basicLineString()); + // make sure that s1, and s2 are between [0, lane_length] + const auto s1_saturated = std::max(0.0, std::min(s1, total_length)); + const auto s2_saturated = std::max(0.0, std::min(s2, total_length)); + + const auto ratio_s1 = s1_saturated / total_length; + const auto ratio_s2 = s2_saturated / total_length; + + const auto s1_left = + static_cast(ratio_s1 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s2_left = + static_cast(ratio_s2 * boost::geometry::length(lanelet.leftBound().basicLineString())); + const auto s1_right = + static_cast(ratio_s1 * boost::geometry::length(lanelet.rightBound().basicLineString())); + const auto s2_right = + static_cast(ratio_s2 * boost::geometry::length(lanelet.rightBound().basicLineString())); + + const auto left_bound = getLineStringFromArcLength(lanelet.leftBound(), s1_left, s2_left); + const auto right_bound = getLineStringFromArcLength(lanelet.rightBound(), s1_right, s2_right); + + return lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +std::optional IntersectionModule::isStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::IntersectionStopLines & intersection_stoplines, + const intersection::PathLanelets & path_lanelets) const +{ + const auto closest_idx = intersection_stoplines.closest_idx; + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + }; + + const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK + const bool stuck_detected = checkStuckVehicleInIntersection(path_lanelets); + const auto first_conflicting_lane = + intersection_lanelets.first_conflicting_lane().value(); // this is OK + const bool is_first_conflicting_lane_private = + (std::string(first_conflicting_lane.attributeOr("location", "else")).compare("private") == 0); + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; + const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; + const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; + if (stuck_detected) { + if ( + is_first_conflicting_lane_private && + planner_param_.stuck_vehicle.disable_against_private_lane) { + // do nothing + } else { + std::optional stopline_idx = std::nullopt; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (default_stopline_idx_opt && fromEgoDist(default_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = default_stopline_idx_opt.value(); + } else if ( + first_attention_stopline_idx_opt && + fromEgoDist(first_attention_stopline_idx_opt.value()) >= 0.0) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return intersection::StuckStop{ + closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + } + } + } + return std::nullopt; +} + +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::TRAILER || + object.classification.at(0).label == + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + return true; + } + return false; +} + +bool IntersectionModule::checkStuckVehicleInIntersection( + const intersection::PathLanelets & path_lanelets) const +{ + using lanelet::utils::getArcCoordinates; + using lanelet::utils::getLaneletLength3d; + using lanelet::utils::getPolygonFromArcLength; + using lanelet::utils::to2D; + + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { + return false; + } + + const auto & objects_ptr = planner_data_->predicted_objects; + + // considering lane change in the intersection, these lanelets are generated from the path + const double stuck_vehicle_detect_dist = planner_param_.stuck_vehicle.stuck_vehicle_detect_dist; + Polygon2d stuck_vehicle_detect_area{}; + if (path_lanelets.conflicting_interval_and_remaining.size() == 0) { + return false; + } + + double target_polygon_length = + getLaneletLength3d(path_lanelets.conflicting_interval_and_remaining); + lanelet::ConstLanelets targets = path_lanelets.conflicting_interval_and_remaining; + if (path_lanelets.next) { + targets.push_back(path_lanelets.next.value()); + const double next_arc_length = + std::min(stuck_vehicle_detect_dist, getLaneletLength3d(path_lanelets.next.value())); + target_polygon_length += next_arc_length; + } + const auto target_polygon = + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); + + if (target_polygon.empty()) { + return false; + } + + for (const auto & p : target_polygon) { + stuck_vehicle_detect_area.outer().emplace_back(p.x(), p.y()); + } + + stuck_vehicle_detect_area.outer().emplace_back(stuck_vehicle_detect_area.outer().front()); + bg::correct(stuck_vehicle_detect_area); + + debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold) { + continue; // not stop vehicle + } + + // check if the footprint is in the stuck detect area + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + const bool is_in_stuck_area = !bg::disjoint(obj_footprint, stuck_vehicle_detect_area); + if (is_in_stuck_area) { + debug_data_.stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + +std::optional IntersectionModule::isYieldStuckStatus( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const intersection::IntersectionStopLines & intersection_stoplines, + const TargetObjects & target_objects) const +{ + const auto closest_idx = intersection_stoplines.closest_idx; + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path.points, closest_idx, index); + }; + const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); // this is OK + const auto first_attention_stopline_idx = + intersection_stoplines.first_attention_stopline.value(); // this is OK + const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; + + const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( + target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding()); + if (yield_stuck_detected) { + std::optional stopline_idx = std::nullopt; + const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; + const bool is_before_first_attention_stopline = + fromEgoDist(first_attention_stopline_idx) >= 0.0; + if (stuck_stopline_idx_opt) { + const bool is_over_stuck_stopline = fromEgoDist(stuck_stopline_idx_opt.value()) < + -planner_param_.common.stopline_overshoot_margin; + if (!is_over_stuck_stopline) { + stopline_idx = stuck_stopline_idx_opt.value(); + } + } + if (!stopline_idx) { + if (is_before_default_stopline) { + stopline_idx = default_stopline_idx; + } else if (is_before_first_attention_stopline) { + stopline_idx = closest_idx; + } + } + if (stopline_idx) { + return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + } + } + return std::nullopt; +} + +bool IntersectionModule::checkYieldStuckVehicleInIntersection( + const TargetObjects & target_objects, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::ConstLanelets & attention_lanelets) const +{ + const bool yield_stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.yield_stuck.turn_direction.straight); + }(); + if (!yield_stuck_detection_direction) { + return false; + } + + const double width = planner_data_->vehicle_info_.vehicle_width_m; + const double stuck_vehicle_vel_thr = + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold; + const double yield_stuck_distance_thr = planner_param_.yield_stuck.distance_threshold; + + LineString2d sparse_intersection_path; + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + for (unsigned i = start; i < end; ++i) { + const auto & point = interpolated_path_info.path.points.at(i).point.pose.position; + const auto yaw = tf2::getYaw(interpolated_path_info.path.points.at(i).point.pose.orientation); + if (turn_direction_ == "right") { + const double right_x = point.x - width / 2 * std::sin(yaw); + const double right_y = point.y + width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(right_x, right_y); + } else if (turn_direction_ == "left") { + const double left_x = point.x + width / 2 * std::sin(yaw); + const double left_y = point.y - width / 2 * std::cos(yaw); + sparse_intersection_path.emplace_back(left_x, left_y); + } else { + // straight + sparse_intersection_path.emplace_back(point.x, point.y); + } + } + lanelet::ConstLanelets yield_stuck_detect_lanelets; + for (const auto & attention_lanelet : attention_lanelets) { + const auto centerline = attention_lanelet.centerline2d().basicLineString(); + std::vector intersects; + bg::intersection(sparse_intersection_path, centerline, intersects); + if (intersects.empty()) { + continue; + } + const auto intersect = intersects.front(); + const auto intersect_arc_coords = lanelet::geometry::toArcCoordinates( + centerline, lanelet::BasicPoint2d(intersect.x(), intersect.y())); + const double yield_stuck_start = + std::max(0.0, intersect_arc_coords.length - yield_stuck_distance_thr); + const double yield_stuck_end = intersect_arc_coords.length; + yield_stuck_detect_lanelets.push_back( + ::createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); + } + debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); + for (const auto & object : target_objects.all_attention_objects) { + const auto obj_v_norm = std::hypot( + object.object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; + } + for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + if (is_in_lanelet) { + debug_data_.yield_stuck_targets.objects.push_back(object.object); + return true; + } + } + } + return false; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index f56d0a4adcff7..ba8b6d86350bc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -51,7 +52,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -122,9 +123,9 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR } const auto first_conflicting_lanelet = first_conflicting_lanelet_.value(); - const auto first_conflicting_idx_opt = getFirstPointInsidePolygonByFootprint( - first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, - planner_data_->vehicle_info_.max_longitudinal_offset_m); + const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto first_conflicting_idx_opt = util::getFirstPointInsidePolygonByFootprint( + first_conflicting_lanelet.polygon3d(), interpolated_path_info, local_footprint, baselink2front); if (!first_conflicting_idx_opt) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 6afc2d61d77f2..dfdfb01fb2234 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -14,14 +14,13 @@ #include "util.hpp" -#include "util_type.hpp" +#include "interpolated_path_info.hpp" #include #include #include #include #include -#include #include #include @@ -29,23 +28,22 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include #include -namespace behavior_velocity_planner +namespace behavior_velocity_planner::util { namespace bg = boost::geometry; -namespace util -{ - static std::optional getDuplicatedPointIdx( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) @@ -135,7 +133,8 @@ std::optional> findLaneIdsInterval( } std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -155,6 +154,34 @@ std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + + for (size_t i = start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>(i, j); + } + } + } + return std::nullopt; +} + std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, @@ -333,12 +360,12 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { - InterpolatedPathInfo interpolated_path_info; + intersection::InterpolatedPathInfo interpolated_path_info; if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger)) { return std::nullopt; } @@ -367,5 +394,14 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -} // namespace util -} // namespace behavior_velocity_planner +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec) +{ + std::vector polys; + for (auto && ll : ll_vec) { + polys.push_back(ll.polygon3d()); + } + return polys; +} + +} // namespace behavior_velocity_planner::util diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 86a34d1e95114..f103191b2dc6f 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -15,25 +15,22 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include "util_type.hpp" +#include "interpolated_path_info.hpp" -#include +#include +#include -#include -#include +#include + +#include +#include -#include -#include #include #include -#include -#include #include #include -namespace behavior_velocity_planner -{ -namespace util +namespace behavior_velocity_planner::util { /** @@ -111,7 +108,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); * @fn * @brief interpolate PathWithLaneId */ -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); @@ -139,10 +136,20 @@ mergeLaneletsByTopologicalSort( * polygon */ std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, + const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); -} // namespace util -} // namespace behavior_velocity_planner +std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const intersection::InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); + +std::vector getPolygon3dFromLanelets( + const lanelet::ConstLanelets & ll_vec); + +} // namespace behavior_velocity_planner::util #endif // UTIL_HPP_ From 7943a8b1ac8888c94cc4aab968a213b7a7d2bca7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 29 Jan 2024 10:04:53 +0900 Subject: [PATCH 21/36] feat(intersection): publish and visualize the reason for dangerous situation to blame past detection fault retrospectively (#6143) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 2 + .../config/intersection.param.yaml | 2 + .../package.xml | 3 +- .../src/debug.cpp | 122 ++- .../src/decision_result.cpp | 65 ++ .../src/decision_result.hpp | 67 +- .../src/interpolated_path_info.hpp | 1 - .../src/intersection_lanelets.hpp | 2 - .../src/intersection_stoplines.hpp | 5 +- .../src/manager.cpp | 5 + .../src/object_manager.cpp | 309 ++++++++ .../src/object_manager.hpp | 294 +++++++ .../src/result.hpp | 17 +- .../src/scene_intersection.cpp | 559 ++++++------- .../src/scene_intersection.hpp | 233 +++--- .../src/scene_intersection_collision.cpp | 746 ++++++++++++++++-- .../src/scene_intersection_occlusion.cpp | 49 +- .../src/scene_intersection_prepare_data.cpp | 95 ++- .../src/scene_intersection_stuck.cpp | 26 +- .../src/util.hpp | 14 +- 20 files changed, 1928 insertions(+), 688 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/src/decision_result.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/object_manager.cpp create mode 100644 planning/behavior_velocity_intersection_module/src/object_manager.hpp diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 4c8fe5c6fa0f5..07847a08c1209 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -12,6 +12,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/util.cpp src/scene_intersection.cpp src/intersection_lanelets.cpp + src/object_manager.cpp + src/decision_result.cpp src/scene_intersection_prepare_data.cpp src/scene_intersection_stuck.cpp src/scene_intersection_occlusion.cpp diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 108e021240851..c7f6f62d575a0 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -59,6 +59,8 @@ object_expected_deceleration: 2.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 + avoid_collision_by_acceleration: + object_time_margin_to_collision_point: 4.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0c9b3407d0f38..0bed7d32f412f 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -22,10 +22,12 @@ autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common + fmt geometry_msgs interpolation lanelet2_extension libopencv-dev + magic_enum motion_utils nav_msgs pluginlib @@ -33,7 +35,6 @@ route_handler rtc_interface tf2_geometry_msgs - tier4_api_msgs tier4_autoware_utils tier4_planning_msgs vehicle_info_util diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 350f6d774f7cf..978855b36c5f6 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -31,8 +31,6 @@ #include #include -namespace behavior_velocity_planner -{ namespace { using tier4_autoware_utils::appendMarkerArray; @@ -40,14 +38,14 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; -static visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( +visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( const std::vector & polygons, 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 i = 0; - int32_t uid = planning_utils::bitShift(lane_id); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -158,8 +156,59 @@ visualization_msgs::msg::MarkerArray createLineMarkerArray( return marker_point; } +constexpr std::tuple white() +{ + constexpr uint64_t code = 0xfdfdfd; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple green() +{ + constexpr uint64_t code = 0x5fa641; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple yellow() +{ + constexpr uint64_t code = 0xebce2b; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple red() +{ + constexpr uint64_t code = 0xba1c30; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} + +constexpr std::tuple light_blue() +{ + constexpr uint64_t code = 0x96cde6; + constexpr float r = static_cast(code >> 16) / 255.0; + constexpr float g = static_cast((code << 48) >> 56) / 255.0; + constexpr float b = static_cast((code << 56) >> 56) / 255.0; + return {r, g, b}; +} } // namespace +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; + visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray() { visualization_msgs::msg::MarkerArray debug_marker_array; @@ -168,14 +217,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.attention_area.value(), "attention_area", lane_id_, 0.0, 1.0, 0.0), &debug_marker_array); } if (debug_data_.occlusion_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, 0.568, 0.596), &debug_marker_array); @@ -183,14 +232,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.adjacent_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.adjacent_area.value(), "adjacent_area", lane_id_, 0.913, 0.639, 0.149), &debug_marker_array); } if (debug_data_.first_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.first_attention_area.value()}, "first_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -198,7 +247,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.second_attention_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.second_attention_area.value()}, "second_attention_area", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); @@ -214,7 +263,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.yield_stuck_detect_area) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( debug_data_.yield_stuck_detect_area.value(), "yield_stuck_detect_area", lane_id_, 0.6588235, 0.34509, 0.6588235), &debug_marker_array); @@ -222,7 +271,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.ego_lane) { appendMarkerArray( - createLaneletPolygonsMarkerArray( + ::createLaneletPolygonsMarkerArray( {debug_data_.ego_lane.value()}, "ego_lane", lane_id_, 1, 0.647, 0.0), &debug_marker_array, now); } @@ -235,59 +284,58 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - size_t i{0}; - for (const auto & p : debug_data_.candidate_collision_object_polygons) { - appendMarkerArray( - debug::createPolygonMarkerArray( - p, "candidate_collision_object_polygons", lane_id_ + i++, now, 0.3, 0.0, 0.0, 0.0, 0.5, - 0.5), - &debug_marker_array, now); - } - + static constexpr auto white = ::white(); + static constexpr auto green = ::green(); + static constexpr auto yellow = ::yellow(); + static constexpr auto red = ::red(); + static constexpr auto light_blue = ::light_blue(); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), + debug_data_.safe_under_traffic_control_targets, "safe_under_traffic_control_targets", + module_id_, now, std::get<0>(light_blue), std::get<1>(light_blue), std::get<2>(light_blue)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + debug_data_.unsafe_targets, "unsafe_targets", module_id_, now, std::get<0>(green), + std::get<1>(green), std::get<2>(green)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, - 0.0, 1.0, 0.0), + debug_data_.misjudge_targets, "misjudge_targets", module_id_, now, std::get<0>(yellow), + std::get<1>(yellow), std::get<2>(yellow)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), + debug_data_.too_late_detect_targets, "too_late_detect_targets", module_id_, now, + std::get<0>(red), std::get<1>(red), std::get<2>(red)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + debug_data_.parked_targets, "parked_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); appendMarkerArray( debug::createObjectsMarkerArray( - debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, - 0.99, 0.6), + debug_data_.stuck_targets, "stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - /* appendMarkerArray( - createPoseMarkerArray( - debug_data_.predicted_obj_pose, "predicted_obj_pose", module_id_, 0.7, 0.85, 0.9), + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "yield_stuck_targets", module_id_, now, std::get<0>(white), + std::get<1>(white), std::get<2>(white)), &debug_marker_array, now); - */ if (debug_data_.first_pass_judge_wall_pose) { const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -297,7 +345,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; appendMarkerArray( - createPoseMarkerArray( + ::createPoseMarkerArray( debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, r, g, 0.0), &debug_marker_array, now); @@ -314,7 +362,7 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( if (debug_data_.nearest_occlusion_projection) { const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); appendMarkerArray( - createLineMarkerArray( + ::createLineMarkerArray( point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), &debug_marker_array, now); } @@ -369,11 +417,11 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = planning_utils::bitShift(module_id_); + int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( - createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), + ::createPoseMarkerArray(debug_data_.stop_point_pose, "stop_point_pose", uid, 1.0, 0.0, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp new file mode 100644 index 0000000000000..93a7c2a29d2f7 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -0,0 +1,65 @@ +// Copyright 2024 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 "decision_result.hpp" + +namespace behavior_velocity_planner::intersection +{ +std::string formatDecisionResult(const DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "InternalError because " + state.error; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + + state.evasive_report; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "YieldStuckStop:\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "FirstWaitBeforeOcclusion"; + } + if (std::holds_alternative(decision_result)) { + return "PeekingTowardOcclusion"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OccludedCollisionStop\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + const auto state = std::get(decision_result); + return "FullyPrioritized\nsafety_report:" + state.safety_report; + } + return ""; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp index 48b0ecf1349a5..da71168c2c4ca 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -23,16 +23,23 @@ namespace behavior_velocity_planner::intersection { /** - * @struct - * @brief Internal error or ego already passed pass_judge_line + * @brief internal error */ -struct Indecisive +struct InternalError { std::string error; }; /** - * @struct + * @brief + */ +struct OverPassJudge +{ + std::string safety_report; + std::string evasive_report; +}; + +/** * @brief detected stuck vehicle */ struct StuckStop @@ -43,17 +50,16 @@ struct StuckStop }; /** - * @struct * @brief yielded by vehicle on the attention area */ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief only collision is detected */ struct NonOccludedCollisionStop @@ -61,10 +67,10 @@ struct NonOccludedCollisionStop size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; /** - * @struct * @brief occlusion is detected so ego needs to stop at the default stop line position */ struct FirstWaitBeforeOcclusion @@ -76,7 +82,6 @@ struct FirstWaitBeforeOcclusion }; /** - * @struct * @brief ego is approaching the boundary of attention area in the presence of traffic light */ struct PeekingTowardOcclusion @@ -96,7 +101,6 @@ struct PeekingTowardOcclusion }; /** - * @struct * @brief both collision and occlusion are detected in the presence of traffic light */ struct OccludedCollisionStop @@ -110,10 +114,10 @@ struct OccludedCollisionStop //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; + std::string safety_report; }; /** - * @struct * @brief at least occlusion is detected in the absence of traffic light */ struct OccludedAbsenceTrafficLight @@ -124,10 +128,10 @@ struct OccludedAbsenceTrafficLight size_t closest_idx{0}; size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; + std::string safety_report; }; /** - * @struct * @brief both collision and occlusion are not detected */ struct Safe @@ -138,7 +142,6 @@ struct Safe }; /** - * @struct * @brief traffic light is red or arrow signal */ struct FullyPrioritized @@ -147,10 +150,12 @@ struct FullyPrioritized size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string safety_report; }; using DecisionResult = std::variant< - Indecisive, //! internal process error, or over the pass judge line + InternalError, //! internal process error, or over the pass judge line + OverPassJudge, //! over the pass judge lines StuckStop, //! detected stuck vehicle YieldStuckStop, //! detected yield stuck vehicle NonOccludedCollisionStop, //! detected collision while FOV is clear @@ -162,41 +167,7 @@ using DecisionResult = std::variant< FullyPrioritized //! only detect vehicles violating traffic rules >; -inline std::string formatDecisionResult(const DecisionResult & decision_result) -{ - if (std::holds_alternative(decision_result)) { - const auto indecisive = std::get(decision_result); - return "Indecisive because " + indecisive.error; - } - if (std::holds_alternative(decision_result)) { - return "StuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "YieldStuckStop"; - } - if (std::holds_alternative(decision_result)) { - return "NonOccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedCollisionStop"; - } - if (std::holds_alternative(decision_result)) { - return "OccludedAbsenceTrafficLight"; - } - if (std::holds_alternative(decision_result)) { - return "Safe"; - } - if (std::holds_alternative(decision_result)) { - return "FullyPrioritized"; - } - return ""; -} +std::string formatDecisionResult(const DecisionResult & decision_result); } // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index c47f016fbdbda..9002c88354d68 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -27,7 +27,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief wrapper class of interpolated path with lane id */ struct InterpolatedPathInfo diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 11deae6bdc001..9624d375de122 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -31,7 +31,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionLanelets */ struct IntersectionLanelets @@ -174,7 +173,6 @@ struct IntersectionLanelets }; /** - * @struct * @brief see the document for more details of PathLanelets */ struct PathLanelets diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 64d20b81e3fad..99d79d4468b38 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -21,7 +21,6 @@ namespace behavior_velocity_planner::intersection { /** - * @struct * @brief see the document for more details of IntersectionStopLines */ struct IntersectionStopLines @@ -63,9 +62,9 @@ struct IntersectionStopLines /** * second_pass_judge_line is before second_attention_stopline by the braking distance. if - * second_attention_lane is null, it is same as first_pass_judge_line + * second_attention_lane is null, it is null */ - size_t second_pass_judge_line{0}; + std::optional second_pass_judge_line{std::nullopt}; /** * occlusion_wo_tl_pass_judge_line is null if ego footprint along the path does not intersect with diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 76ccedb7c23a8..79d31352ab911 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -134,6 +134,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + ip.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point = getOrDeclareParameter( + node, + ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp new file mode 100644 index 0000000000000..ea5d89fe8052d --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -0,0 +1,309 @@ +// Copyright 2024 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 "object_manager.hpp" + +#include +#include +#include // for toPolygon2d + +#include +#include +#include + +#include +#include + +#include + +namespace +{ +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} + +tier4_autoware_utils::Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + namespace bg = boost::geometry; + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + tier4_autoware_utils::Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + tier4_autoware_utils::Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} + +} // namespace + +namespace behavior_velocity_planner::intersection +{ +namespace bg = boost::geometry; + +ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_str(::to_string(uuid)) +{ +} + +void ObjectInfo::initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & object, + std::optional attention_lanelet_opt_, + std::optional stopline_opt_) +{ + predicted_object_ = object; + attention_lanelet_opt = attention_lanelet_opt_; + stopline_opt = stopline_opt_; + unsafe_interval_ = std::nullopt; + calc_dist_to_stopline(); +} + +void ObjectInfo::update_safety( + const std::optional & unsafe_interval, + const std::optional & safe_interval, const bool safe_under_traffic_control) +{ + unsafe_interval_ = unsafe_interval; + safe_interval_ = safe_interval; + safe_under_traffic_control_ = safe_under_traffic_control; +} + +std::optional ObjectInfo::estimated_past_position( + const double past_duration) const +{ + if (!attention_lanelet_opt) { + return std::nullopt; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto current_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto distance = current_arc_coords.distance; + const auto past_length = + current_arc_coords.length - + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x * past_duration; + const auto past_point = lanelet::geometry::fromArcCoordinates( + attention_lanelet.centerline2d(), lanelet::ArcCoordinates{past_length, distance}); + geometry_msgs::msg::Point past_position; + past_position.x = past_point.x(); + past_position.y = past_point.y(); + return std::make_optional(past_position); +} + +void ObjectInfo::calc_dist_to_stopline() +{ + if (!stopline_opt || !attention_lanelet_opt) { + return; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose); + const auto stopline = stopline_opt.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0; + stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0; + stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center); + dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length); +} + +bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + return dist_to_stopline > braking_distance; +} + +bool ObjectInfo::can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const +{ + if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + const double observed_velocity = + predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + (observed_velocity * observed_velocity) / (2.0 * brake_deceleration); + if (dist_to_stopline > braking_distance) { + return false; + } + const auto attention_lanelet = attention_lanelet_opt.value(); + const auto stopline = stopline_opt.value(); + const auto stopline_p1 = stopline.front(); + const auto stopline_p2 = stopline.back(); + const tier4_autoware_utils::Point2d stopline_mid{ + stopline_p1.x() + stopline_p2.x() / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; + const auto attention_lane_end = attention_lanelet.centerline().back(); + const tier4_autoware_utils::LineString2d attention_lane_later_part( + {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, + tier4_autoware_utils::Point2d{attention_lane_end.x(), attention_lane_end.y()}}); + std::vector ego_collision_points; + bg::intersection( + attention_lane_later_part, ego_lane.centerline2d().basicLineString(), ego_collision_points); + if (ego_collision_points.empty()) { + return false; + } + const auto expected_collision_point = ego_collision_points.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; + const double stopline_to_ego_path = std::hypot( + expected_collision_point.x() - stopline_mid.x(), + expected_collision_point.y() - stopline_mid.y()); + const double object_to_ego_path = stopline_to_ego_path - stopline_to_object; + // NOTE: if object_to_ego_path < 0, object passed ego path + return object_to_ego_path > tolerable_overshoot; +} + +bool ObjectInfo::before_stopline_by(const double margin) const +{ + if (!dist_to_stopline_opt) { + return false; + } + const double dist_to_stopline = dist_to_stopline_opt.value(); + return dist_to_stopline < margin; +} + +std::shared_ptr ObjectInfoManager::registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle) +{ + if (objects_info_.count(uuid) == 0) { + auto object = std::make_shared(uuid); + objects_info_[uuid] = object; + } + auto object = objects_info_[uuid]; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } + return object; +} + +void ObjectInfoManager::registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked_vehicle, + std::shared_ptr object) +{ + objects_info_[uuid] = object; + if (belong_attention_area) { + attention_area_objects_.push_back(object); + } else if (belong_intersection_area) { + intersection_area_objects_.push_back(object); + } + if (is_parked_vehicle) { + parked_objects_.push_back(object); + } +} + +void ObjectInfoManager::clearObjects() +{ + objects_info_.clear(); + attention_area_objects_.clear(); + intersection_area_objects_.clear(); + parked_objects_.clear(); +}; + +std::vector> ObjectInfoManager::allObjects() +{ + std::vector> all_objects = attention_area_objects_; + all_objects.insert( + all_objects.end(), intersection_area_objects_.begin(), intersection_area_objects_.end()); + all_objects.insert(all_objects.end(), parked_objects_.begin(), parked_objects_.end()); + return all_objects; +} + +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt) +{ + const auto first_itr = std::adjacent_find( + predicted_path.path.cbegin(), predicted_path.path.cend(), [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (first_itr == predicted_path.path.cend()) { + // even the predicted path end does not collide with the beginning of ego_lane_poly + return std::nullopt; + } + const auto last_itr = std::adjacent_find( + predicted_path.path.crbegin(), predicted_path.path.crend(), + [&](const auto & a, const auto & b) { + return bg::intersects(ego_lane_poly, ::createOneStepPolygon(a, b, shape)); + }); + if (last_itr == predicted_path.path.crend()) { + // even the predicted path start does not collide with the end of ego_lane_poly + return std::nullopt; + } + + const size_t enter_idx = static_cast(first_itr - predicted_path.path.begin()); + const double object_enter_time = + static_cast(enter_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const size_t exit_idx = std::distance(predicted_path.path.begin(), last_itr.base()) - 1; + const double object_exit_time = + static_cast(exit_idx) * rclcpp::Duration(predicted_path.time_step).seconds(); + const auto lane_position = [&]() { + if (first_attention_lane_opt) { + if (lanelet::geometry::inside( + first_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::FIRST; + } + } + if (second_attention_lane_opt) { + if (lanelet::geometry::inside( + second_attention_lane_opt.value(), + lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { + return intersection::CollisionInterval::LanePosition::SECOND; + } + } + return intersection::CollisionInterval::LanePosition::ELSE; + }(); + + std::vector path; + for (const auto & pose : predicted_path.path) { + path.push_back(pose); + } + return intersection::CollisionInterval{ + lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; +} + +} // namespace behavior_velocity_planner::intersection diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp new file mode 100644 index 0000000000000..e77849570cda8 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -0,0 +1,294 @@ +// Copyright 2024 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 OBJECT_MANAGER_HPP_ +#define OBJECT_MANAGER_HPP_ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash +{ + size_t operator()(const unique_identifier_msgs::msg::UUID & uid) const + { + const auto & ids = uid.uuid; + boost::uuids::uuid u = {ids[0], ids[1], ids[2], ids[3], ids[4], ids[5], ids[6], ids[7], + ids[8], ids[9], ids[10], ids[11], ids[12], ids[13], ids[14], ids[15]}; + return boost::hash()(u); + } +}; +} // namespace std + +namespace behavior_velocity_planner::intersection +{ + +/** + * @brief store collision information + */ +struct CollisionInterval +{ + enum LanePosition { + FIRST, + SECOND, + ELSE, + }; + LanePosition lane_position{LanePosition::ELSE}; + + //! original predicted path + std::vector path; + + //! possible collision interval position index on path + std::pair interval_position; + + //! possible collision interval time(without TTC margin) + std::pair interval_time; +}; + +struct CollisionKnowledge +{ + //! the time when the expected collision is judged + rclcpp::Time stamp; + + enum SafeType { + UNSAFE, + SAFE, + SAFE_UNDER_TRAFFIC_CONTROL, + }; + SafeType safe_type{SafeType::UNSAFE}; + + //! if !safe, this has value, and it safe, this maybe null if the predicted path does not + //! intersect with ego path + std::optional interval{std::nullopt}; + + double observed_velocity; +}; + +/** + * @brief store collision information of object on the attention area + */ +class ObjectInfo +{ +public: + explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); + + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + { + return predicted_object_; + }; + + std::optional is_unsafe() const + { + if (safe_under_traffic_control_) { + return std::nullopt; + } + if (!unsafe_interval_) { + return std::nullopt; + } + return unsafe_interval_; + } + + bool is_safe_under_traffic_control() const { return safe_under_traffic_control_; } + + /** + * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline + */ + void initialize( + const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + std::optional attention_lanelet_opt, + std::optional stopline_opt); + + /** + * @brief update unsafe_knowledge + */ + void update_safety( + const std::optional & unsafe_interval_opt, + const std::optional & safe_interval_opt, + const bool safe_under_traffic_control); + + /** + * @brief find the estimated position of the object in the past + */ + std::optional estimated_past_position( + const double past_duration) const; + + /** + * @brief check if object can stop before stopline under the deceleration. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_stopline(const double brake_deceleration) const; + + /** + * @brief check if object can stop before stopline within the overshoot margin. return false if + * stopline is null for conservative collision checking + */ + bool can_stop_before_ego_lane( + const double brake_deceleration, const double tolerable_overshoot, + lanelet::ConstLanelet ego_lane) const; + + /** + * @brief check if the object is before the stopline within the specified margin + */ + bool before_stopline_by(const double margin) const; + + void setDecisionAt1stPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_1st_pass_judge_line_passage_ = knowledge; + } + + void setDecisionAt2ndPassJudgeLinePassage(const CollisionKnowledge & knowledge) + { + decision_at_2nd_pass_judge_line_passage_ = knowledge; + } + + const std::optional & unsafe_interval() const { return unsafe_interval_; } + + double observed_velocity() const + { + return predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x; + } + + const std::optional & decision_at_1st_pass_judge_line_passage() const + { + return decision_at_1st_pass_judge_line_passage_; + } + + const std::optional & decision_at_2nd_pass_judge_line_passage() const + { + return decision_at_2nd_pass_judge_line_passage_; + } + + const std::string uuid_str; + +private: + autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + + //! null if the object in intersection_area but not in attention_area + std::optional attention_lanelet_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional stopline_opt{std::nullopt}; + + //! null if the object in intersection_area but not in attention_area + std::optional dist_to_stopline_opt{std::nullopt}; + + //! store the information if judged as UNSAFE + std::optional unsafe_interval_{std::nullopt}; + + //! store the information if judged as SAFE + std::optional safe_interval_{std::nullopt}; + + //! true if the object is judged as negligible given traffic light color + bool safe_under_traffic_control_{false}; + + std::optional decision_at_1st_pass_judge_line_passage_{std::nullopt}; + std::optional decision_at_2nd_pass_judge_line_passage_{std::nullopt}; + + /** + * @brief calculate/update the distance to corresponding stopline + */ + void calc_dist_to_stopline(); +}; + +/** + * @brief store predicted objects for intersection + */ +class ObjectInfoManager +{ +public: + std::shared_ptr registerObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked); + + void registerExistingObject( + const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, + const bool belong_intersection_area, const bool is_parked, + std::shared_ptr object); + + void clearObjects(); + + const std::vector> & attentionObjects() const + { + return attention_area_objects_; + } + + const std::vector> & parkedObjects() const { return parked_objects_; } + + std::vector> allObjects(); + + const std::unordered_map> & + getObjectsMap() + { + return objects_info_; + } + + void setPassed1stPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_1st_judge_line_first_time_ = time; + } + void setPassed2ndPassJudgeLineFirstTime(const rclcpp::Time & time) + { + passed_2nd_judge_line_first_time_ = time; + } + +private: + std::unordered_map> objects_info_; + + //! belong to attention area + std::vector> attention_area_objects_; + + //! does not belong to attention area but to intersection area + std::vector> intersection_area_objects_; + + //! parked objects on attention_area/intersection_area + std::vector> parked_objects_; + + std::optional passed_1st_judge_line_first_time_{std::nullopt}; + std::optional passed_2nd_judge_line_first_time_{std::nullopt}; +}; + +/** + * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically + */ +std::optional findPassageInterval( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_auto_perception_msgs::msg::Shape & shape, + const lanelet::BasicPolygon2d & ego_lane_poly, + const std::optional & first_attention_lane_opt, + const std::optional & second_attention_lane_opt); + +} // namespace behavior_velocity_planner::intersection + +#endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/behavior_velocity_intersection_module/src/result.hpp index cc6ad880b8153..5d82183cee2fb 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/behavior_velocity_intersection_module/src/result.hpp @@ -15,6 +15,7 @@ #ifndef RESULT_HPP_ #define RESULT_HPP_ +#include #include namespace behavior_velocity_planner::intersection @@ -23,10 +24,6 @@ namespace behavior_velocity_planner::intersection template class Result { -public: - static Result make_ok(const Ok & ok) { return Result(ok); } - static Result make_err(const Error & err) { return Result(err); } - public: explicit Result(const Ok & ok) : data_(ok) {} explicit Result(const Error & err) : data_(err) {} @@ -39,6 +36,18 @@ class Result std::variant data_; }; +template +Result make_ok(Args &&... args) +{ + return Result(Ok{std::forward(args)...}); +} + +template +Result make_err(Args &&... args) +{ + return Result(Error{std::forward(args)...}); +} + } // namespace behavior_velocity_planner::intersection #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 90242dc3edd7e..75e2da034780a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -40,6 +40,10 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, @@ -95,10 +99,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_ = DebugData(); *stop_reason = planning_utils::initializeStopReason(StopReason::INTERSECTION); - // set default RTC initializeRTCStatus(); - // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); prev_decision_result_ = decision_result; @@ -112,7 +114,6 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * reactRTCApproval(decision_result, path, stop_reason); - RCLCPP_DEBUG(logger_, "===== plan end ====="); return true; } @@ -141,133 +142,145 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto [interpolated_path_info, intersection_stoplines, path_lanelets] = prepare_data.ok(); const auto & intersection_lanelets = intersection_lanelets_.value(); - const auto closest_idx = intersection_stoplines.closest_idx; - - // utility functions - auto fromEgoDist = [&](const size_t index) { - return motion_utils::calcSignedArcLength(path->points, closest_idx, index); - }; - auto stoppedForDuration = - [&](const size_t pos, const double duration, StateMachine & state_machine) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < 0.0); - const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { - state_machine.setState(StateMachine::State::GO); - } - return state_machine.getState() == StateMachine::State::GO; - }; - auto stoppedAtPosition = [&](const size_t pos, const double duration) { - const double dist_stopline = fromEgoDist(pos); - const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; - }; - + // ========================================================================================== + // stuck detection + // // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation + // ========================================================================================== const auto is_stuck_status = isStuckStatus(*path, intersection_stoplines, path_lanelets); if (is_stuck_status) { return is_stuck_status.value(); } + // ========================================================================================== + // basic data validation + // // if attention area is empty, collision/occlusion detection is impossible + // + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line, so ego is already in the middle of the intersection, or the + // end of the ego path has just entered the entry of this intersection + // + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, ego has already passed the intersection, or the end of the + // ego path has just entered the entry of this intersection + // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::Indecisive{"attention area is empty"}; + return intersection::InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); - // if attention area is not null but default stop line is not available, ego/backward-path has - // already passed the stop line const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::Indecisive{"default stop line is null"}; + return intersection::InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); - // occlusion stop line is generated from the intersection of ego footprint along the path with the - // attention area, so if this is null, eog has already passed the intersection const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::Indecisive{"occlusion stop line is null"}; + return intersection::InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); - debug_data_.attention_area = intersection_lanelets.attention_area(); - debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); - debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); - debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + // ========================================================================================== + // classify the objects to attention_area/intersection_area and update their position, velocity, + // belonging attention lanelet, distance to corresponding stopline + // ========================================================================================== + updateObjectInfoManagerArea(); + + // ========================================================================================== + // occlusion_status is type of occlusion, + // is_occlusion_cleared_with_margin indicates if occlusion is physically detected + // is_occlusion_state indicates if occlusion is detected. OR occlusion is not detected but RTC for + // intersection_occlusion is disapproved, which means ego is virtually occluded + // + // so is_occlusion_cleared_with_margin should be sent to RTC as module decision + // and is_occlusion_status should be only used to decide ego action + // !is_occlusion_state == !physically_occluded && !externally_occluded, so even if occlusion is + // not detected but not approved, SAFE is not sent. + // ========================================================================================== + const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = + getOcclusionStatus(traffic_prioritized_level, interpolated_path_info); - // get intersection area - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); - const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); - // filter objects - auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const auto is_yield_stuck_status = - isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines, target_objects); - if (is_yield_stuck_status) { - return is_yield_stuck_status.value(); + const auto + [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line] = + isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); + + // ========================================================================================== + // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the + // exit of intersection + // ========================================================================================== + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + const auto time_distance_array = + calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); + + // ========================================================================================== + // run collision checking for each objects considering traffic light level. Also if ego just + // passed each pass judge line for the first time, save current collision status for late + // diagnosis + // ========================================================================================== + updateObjectInfoManagerCollision( + path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, + safely_passed_2nd_judge_line); + + const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] = + detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); + const std::string safety_diag = + generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects); + if (is_permanent_go_) { + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{safety_diag, evasive_diag}; + } + return intersection::OverPassJudge{ + "no collision is detected", "ego can safely pass the intersection at this rate"}; } - const auto [occlusion_status, is_occlusion_cleared_with_margin, is_occlusion_state] = - getOcclusionStatus( - traffic_prioritized_level, interpolated_path_info, intersection_lanelets, target_objects); - - // TODO(Mamoru Sobue): this should be called later for safety diagnosis - const auto is_over_pass_judge_lines_status = - isOverPassJudgeLinesStatus(*path, is_occlusion_state, intersection_stoplines); - if (is_over_pass_judge_lines_status) { - return is_over_pass_judge_lines_status.ok(); + const bool collision_on_1st_attention_lane = + has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + // ========================================================================================== + // this state is very dangerous because ego is very close/over the boundary of 1st attention lane + // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this + // case, possible another collision may be expected on the 2nd attention lane too. + // ========================================================================================== + std::string safety_report = safety_diag; + if ( + is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line && + is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { + safety_report += + "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st " + "attention lane, which is dangerous."; } - [[maybe_unused]] const auto [is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line] = - is_over_pass_judge_lines_status.err(); - const auto & current_pose = planner_data_->current_odometry->pose; - const bool is_over_default_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto closest_idx = intersection_stoplines.closest_idx; + const bool is_over_default_stopline = util::isOverTargetIndex( + *path, closest_idx, planner_data_->current_odometry->pose, default_stopline_idx); const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; - const auto is_green_pseudo_collision_status = isGreenPseudoCollisionStatus( - *path, collision_stopline_idx, intersection_stoplines, target_objects); + // ========================================================================================== + // pseudo collision detection on green light + // ========================================================================================== + const auto is_green_pseudo_collision_status = + isGreenPseudoCollisionStatus(closest_idx, collision_stopline_idx, intersection_stoplines); if (is_green_pseudo_collision_status) { return is_green_pseudo_collision_status.value(); } - // if ego is waiting for collision detection, the entry time into the intersection is a bit - // delayed for the chattering hold - const bool is_go_out = (activated_ && occlusion_activated_); - const double time_to_restart = - (is_go_out || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.collision_detection_hold_time - - collision_state_machine_.getDuration()); - - // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd - // pass judge line respectively, ego should go - const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; - const auto last_intersection_stopline_candidate_idx = - second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - *path, closest_idx, last_intersection_stopline_candidate_idx, time_to_restart, - &ego_ttc_time_array); + // ========================================================================================== + // yield stuck detection + // ========================================================================================== + const auto is_yield_stuck_status = + isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); + if (is_yield_stuck_status) { + auto yield_stuck = is_yield_stuck_status.value(); + yield_stuck.safety_report = safety_report; + return yield_stuck; + } - const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, - time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -276,7 +289,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (is_prioritized) { return intersection::FullyPrioritized{ - has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, + safety_report}; } // Safe @@ -286,18 +300,51 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // Only collision if (!is_occlusion_state && has_collision_with_margin) { return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Occluded - // occlusion_status is assured to be not NOT_OCCLUDED + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; + const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; const bool stopped_at_default_line = stoppedForDuration( default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, before_creep_state_machine_); if (stopped_at_default_line) { + // ========================================================================================== // if specified the parameter occlusion.temporal_stop_before_attention_area OR // has_no_traffic_light_, ego will temporarily stop before entering attention area + // ========================================================================================== const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -307,7 +354,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::Indecisive{ + return intersection::InternalError{ "already passed maximum peeking line in the absence of traffic light"}; } return intersection::OccludedAbsenceTrafficLight{ @@ -316,10 +363,15 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stopline_idx, - occlusion_wo_tl_pass_judge_line_idx}; + occlusion_wo_tl_pass_judge_line_idx, + safety_report}; } + + // ========================================================================================== // following remaining block is "has_traffic_light_" + // // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + // ========================================================================================== const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; const bool is_stuck_by_static_occlusion = stoppedAtPosition( @@ -355,7 +407,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_stopline_idx, first_attention_stopline_idx, occlusion_stopline_idx, - static_occlusion_timeout}; + static_occlusion_timeout, + safety_report}; } else { return intersection::PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, @@ -398,7 +451,17 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::Indecisive & result, + [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + return; +} + +template <> +void prepareRTCByDecisionResult( + [[maybe_unused]] const intersection::OverPassJudge & result, [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -606,7 +669,22 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::Indecisive & decision_result, + [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + [[maybe_unused]] const double baselink2front, + [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] StopReason * stop_reason, + [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] IntersectionModule::DebugData * debug_data) +{ + return; +} + +template <> +void reactRTCApprovalByDecisionResult( + [[maybe_unused]] const bool rtc_default_approved, + [[maybe_unused]] const bool rtc_occlusion_approved, + [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, @@ -640,7 +718,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -689,7 +767,7 @@ void reactRTCApprovalByDecisionResult( { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; - stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->unsafe_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, @@ -807,7 +885,6 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required @@ -1125,84 +1202,7 @@ IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPriori return TrafficPrioritizedLevel::NOT_PRIORITIZED; } -TargetObjects IntersectionModule::generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const -{ - const auto & objects_ptr = planner_data_->predicted_objects; - // extract target objects - TargetObjects target_objects; - target_objects.header = objects_ptr->header; - const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); - for (const auto & object : objects_ptr->objects) { - // ignore non-vehicle type objects, such as pedestrian. - if (!isTargetCollisionVehicleType(object)) { - continue; - } - - // check direction of objects - const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto belong_adjacent_lanelet_id = - checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); - if (belong_adjacent_lanelet_id) { - continue; - } - - const auto is_parked_vehicle = - std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; - auto & container = is_parked_vehicle ? target_objects.parked_attention_objects - : target_objects.attention_objects; - if (intersection_area) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); - const auto intersection_area_2d = intersection_area.value(); - const auto belong_attention_lanelet_id = - checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); - if (belong_attention_lanelet_id) { - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = std::nullopt; - target_object.stopline = std::nullopt; - target_objects.intersection_area_objects.push_back(target_object); - } - } else if (const auto belong_attention_lanelet_id = checkAngleForTargetLanelets( - object_direction, attention_lanelets, is_parked_vehicle); - belong_attention_lanelet_id.has_value()) { - // intersection_area is not available, use detection_area_with_margin as before - const auto id = belong_attention_lanelet_id.value(); - TargetObject target_object; - target_object.object = object; - target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stopline = attention_lanelet_stoplines.at(id); - container.push_back(target_object); - } - } - for (const auto & object : target_objects.attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all_attention_objects.push_back(object); - } - for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stopline(); - } - return target_objects; -} - -intersection::Result< - intersection::Indecisive, - std::pair> -IntersectionModule::isOverPassJudgeLinesStatus( +IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { @@ -1216,8 +1216,11 @@ IntersectionModule::isOverPassJudgeLinesStatus( const size_t pass_judge_line_idx = [&]() { if (planner_param_.occlusion.enable) { if (has_traffic_light_) { + // ========================================================================================== // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its - // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + // position is changed to occlusion_stopline_idx. even if occlusion is cleared by peeking, + // its position should be occlusion_stopline_idx as before + // ========================================================================================== if (passed_1st_judge_line_while_peeking_) { return occlusion_stopline_idx; } @@ -1226,16 +1229,22 @@ IntersectionModule::isOverPassJudgeLinesStatus( if (is_occlusion_state && is_over_first_pass_judge_line) { passed_1st_judge_line_while_peeking_ = true; return occlusion_stopline_idx; - } else { - return first_pass_judge_line_idx; } + // ========================================================================================== + // Otherwise it is first_pass_judge_line + // ========================================================================================== + return first_pass_judge_line_idx; } else if (is_occlusion_state) { + // ========================================================================================== // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area + // ========================================================================================== return occlusion_wo_tl_pass_judge_line_idx; } else { + // ========================================================================================== // if there is no traffic light and occlusion is not detected, pass_judge position is - // default + // default position + // ========================================================================================== return first_pass_judge_line_idx; } } @@ -1246,147 +1255,67 @@ IntersectionModule::isOverPassJudgeLinesStatus( const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); + bool safely_passed_1st_judge_line_first_time = false; if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { - safely_passed_1st_judge_line_time_ = clock_->now(); + safely_passed_1st_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_1st_judge_line_first_time = true; } const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.first_pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, path); debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); - const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; - const bool is_over_2nd_pass_judge_line = - util::isOverTargetIndex(path, closest_idx, current_pose, second_pass_judge_line_idx); - if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { - safely_passed_2nd_judge_line_time_ = clock_->now(); + const auto second_pass_judge_line_idx_opt = intersection_stoplines.second_pass_judge_line; + const std::optional is_over_2nd_pass_judge_line = + second_pass_judge_line_idx_opt + ? std::make_optional(util::isOverTargetIndex( + path, closest_idx, current_pose, second_pass_judge_line_idx_opt.value())) + : std::nullopt; + bool safely_passed_2nd_judge_line_first_time = false; + if ( + is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value() && was_safe && + !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = std::make_pair(clock_->now(), current_pose); + safely_passed_2nd_judge_line_first_time = true; + } + if (second_pass_judge_line_idx_opt) { + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx_opt.value(), baselink2front, path); } - debug_data_.second_pass_judge_wall_pose = - planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, path); debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); const bool is_over_default_stopline = util::isOverTargetIndex(path, closest_idx, current_pose, default_stopline_idx); + + const bool over_default_stopline_for_pass_judge = + is_over_default_stopline || planner_param_.common.enable_pass_judge_before_default_stopline; + const bool over_pass_judge_line_overall = + is_over_2nd_pass_judge_line ? is_over_2nd_pass_judge_line.value() : is_over_1st_pass_judge_line; if ( - ((is_over_default_stopline || - planner_param_.common.enable_pass_judge_before_default_stopline) && - is_over_2nd_pass_judge_line && was_safe) || + (over_default_stopline_for_pass_judge && over_pass_judge_line_overall && was_safe) || is_permanent_go_) { - /* - * This body is active if ego is - * - over the default stopline AND - * - over the 1st && 2nd pass judge line AND - * - previously safe - * , - * which means ego can stop even if it is over the 1st pass judge line but - * - before default stopline OR - * - before the 2nd pass judge line OR - * - or previously unsafe - * . - * In order for ego to continue peeking or collision detection when occlusion is detected after - * ego passed the 1st pass judge line, it needs to be - * - before the default stopline OR - * - before the 2nd pass judge line OR - * - previously unsafe - */ + // ========================================================================================== + // this body is active if ego is + // - over the default stopline AND + // - over the 1st && 2nd pass judge line AND + // - previously safe + // , + // which means ego can stop even if it is over the 1st pass judge line but + // - before default stopline OR + // - before the 2nd pass judge line OR + // - or previously unsafe + // . + // + // in order for ego to continue peeking or collision detection when occlusion is detected + // after ego passed the 1st pass judge line, it needs to be + // - before the default stopline OR + // - before the 2nd pass judge line OR + // - previously unsafe + // ========================================================================================== is_permanent_go_ = true; - return intersection::Result>::make_ok( - intersection::Indecisive{"over the pass judge line. no plan needed"}); - } - return intersection::Result>::make_err( - std::make_pair(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line)); -} - -void TargetObject::calc_dist_to_stopline() -{ - if (!attention_lanelet || !stopline) { - return; } - const auto attention_lanelet_val = attention_lanelet.value(); - const auto object_arc_coords = lanelet::utils::getArcCoordinates( - {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stopline_val = stopline.value(); - geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; - stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; - stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; - const auto stopline_arc_coords = - lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); + return { + is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line, + safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -/* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel) - { - const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; - // consider vehicle in ego-lane && in front of ego - const auto lon_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double object_decel = - planner_param_.stuck_vehicle.assumed_front_car_decel; // NOTE: this is positive - const double stopping_distance = lon_vel * lon_vel / (2 * object_decel); - - std::vector center_points; - for (auto && p : ego_lane_with_next_lane[0].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - for (auto && p : ego_lane_with_next_lane[1].centerline()) - center_points.push_back(std::move(lanelet::utils::conversion::toGeomMsgPt(p))); - const double lat_offset = - std::fabs(motion_utils::calcLateralOffset(center_points, object_pose.position)); - // get the nearest centerpoint to object - std::vector dist_obj_center_points; - for (const auto & p : center_points) - dist_obj_center_points.push_back(tier4_autoware_utils::calcDistance2d(object_pose.position, - p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_points.begin(), - std::min_element(dist_obj_center_points.begin(), dist_obj_center_points.end())); - // find two center_points whose distances from `closest_centerpoint` cross stopping_distance - double acc_dist_prev = 0.0, acc_dist = 0.0; - auto p1 = center_points[obj_closest_centerpoint_idx]; - auto p2 = center_points[obj_closest_centerpoint_idx]; - for (unsigned i = obj_closest_centerpoint_idx; i < center_points.size() - 1; ++i) { - p1 = center_points[i]; - p2 = center_points[i + 1]; - acc_dist_prev = acc_dist; - const auto arc_position_p1 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p1)); - const auto arc_position_p2 = - lanelet::utils::getArcCoordinates(ego_lane_with_next_lane, toPose(p2)); - const double delta = arc_position_p2.length - arc_position_p1.length; - acc_dist += delta; - if (acc_dist > stopping_distance) { - break; - } - } - // if stopping_distance >= center_points, stopping_point is center_points[end] - const double ratio = (acc_dist <= stopping_distance) - ? 0.0 - : (acc_dist - stopping_distance) / (stopping_distance - acc_dist_prev); - // linear interpolation - geometry_msgs::msg::Point stopping_point; - stopping_point.x = (p1.x * ratio + p2.x) / (1 + ratio); - stopping_point.y = (p1.y * ratio + p2.y) / (1 + ratio); - stopping_point.z = (p1.z * ratio + p2.z) / (1 + ratio); - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, stopping_point); - stopping_point.x += lat_offset * std::cos(lane_yaw + M_PI / 2.0); - stopping_point.y += lat_offset * std::sin(lane_yaw + M_PI / 2.0); - - // calculate footprint of predicted stopping pose - autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; - predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; - predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - auto predicted_obj_footprint = tier4_autoware_utils::toPolygon2d(predicted_object); - const bool is_in_stuck_area = !bg::disjoint(predicted_obj_footprint, stuck_vehicle_detect_area); - debug_data_.predicted_obj_pose.position = stopping_point; - debug_data_.predicted_obj_pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); - - if (is_in_stuck_area) { - return true; - } - return false; - } -*/ - } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8a34aabe8b918..9527ce8e5ebea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -19,6 +19,7 @@ #include "interpolated_path_info.hpp" #include "intersection_lanelets.hpp" #include "intersection_stoplines.hpp" +#include "object_manager.hpp" #include "result.hpp" #include @@ -45,32 +46,6 @@ namespace behavior_velocity_planner { -/** - * @struct - * @brief see the document for more details of IntersectionStopLines - */ -struct TargetObject -{ - autoware_auto_perception_msgs::msg::PredictedObject object; - std::optional attention_lanelet{std::nullopt}; - std::optional stopline{std::nullopt}; - std::optional dist_to_stopline{std::nullopt}; - void calc_dist_to_stopline(); -}; - -/** - * @struct - * @brief categorize TargetObjects - */ -struct TargetObjects -{ - std_msgs::msg::Header header; - std::vector attention_objects; - std::vector parked_attention_objects; - std::vector intersection_area_objects; - std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy -}; - class IntersectionModule : public SceneModuleInterface { public: @@ -154,6 +129,10 @@ class IntersectionModule : public SceneModuleInterface { double object_margin_to_path; } ignore_on_red_traffic_light; + struct AvoidCollisionByAcceleration + { + double object_time_margin_to_collision_point; + } avoid_collision_by_acceleration; } collision_detection; struct Occlusion @@ -204,36 +183,37 @@ class IntersectionModule : public SceneModuleInterface std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional first_pass_judge_wall_pose{std::nullopt}; + std::optional second_pass_judge_wall_pose{std::nullopt}; bool passed_first_pass_judge{false}; bool passed_second_pass_judge{false}; - std::optional second_pass_judge_wall_pose{std::nullopt}; + std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional first_attention_area{std::nullopt}; std::optional second_attention_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional> yield_stuck_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; - std::optional absence_traffic_light_creep_wall{std::nullopt}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; using TimeDistanceArray = std::vector>; /** - * @struct * @brief categorize traffic light priority */ enum class TrafficPrioritizedLevel { @@ -246,6 +226,41 @@ class IntersectionModule : public SceneModuleInterface }; /** @} */ + /** + * @brief + */ + struct PassJudgeStatus + { + //! true if ego is over the 1st pass judge line + const bool is_over_1st_pass_judge; + + //! true if second_attention_lane exists and ego is over the 2nd pass judge line + const std::optional is_over_2nd_pass_judge; + + //! true only when ego passed 1st pass judge line safely for the first time + const bool safely_passed_1st_judge_line; + + //! true only when ego passed 2nd pass judge line safely for the first time + const bool safely_passed_2nd_judge_line; + }; + + /** + * @brief + */ + struct CollisionStatus + { + enum BlameType { + BLAME_AT_FIRST_PASS_JUDGE, + BLAME_AT_SECOND_PASS_JUDGE, + }; + const bool collision_detected; + const intersection::CollisionInterval::LanePosition collision_position; + const std::vector>> + too_late_detect_objects; + const std::vector>> + misjudge_objects; + }; + IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, @@ -325,6 +340,9 @@ class IntersectionModule : public SceneModuleInterface //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ std::nullopt}; + + //! save the time when ego observed green traffic light before entering the intersection + std::optional initial_green_light_observed_time_{std::nullopt}; /** @}*/ private: @@ -340,17 +358,19 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::Indecisive{""}}; + intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline bool passed_1st_judge_line_while_peeking_{false}; - //! save the time when ego passed the 1st/2nd_pass_judge_line with safe decision. If collision is - //! expected after these variables are non-null, then it is the fault of past perception failure - //! at these time. - std::optional safely_passed_1st_judge_line_time_{std::nullopt}; - std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; + //! save the time and ego position when ego passed the 1st/2nd_pass_judge_line with safe + //! decision. If collision is expected after these variables are non-null, then it is the fault of + //! past perception failure at these time. + std::optional> + safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional> + safely_passed_2nd_judge_line_time_{std::nullopt}; /** @}*/ private: @@ -363,6 +383,9 @@ class IntersectionModule : public SceneModuleInterface */ //! debouncing for stable SAFE decision StateMachine collision_state_machine_; + + //! container for storing safety status of objects on the attention area + intersection::ObjectInfoManager object_info_manager_; /** @} */ private: @@ -388,8 +411,6 @@ class IntersectionModule : public SceneModuleInterface StateMachine static_occlusion_timeout_state_machine_; /** @} */ - std::optional initial_green_light_observed_time_{std::nullopt}; - private: /** *********************************************************** @@ -464,13 +485,13 @@ class IntersectionModule : public SceneModuleInterface /** * @brief prepare basic data structure - * @return return IntersectionStopLines if all data is valid, otherwise Indecisive + * @return return IntersectionStopLines if all data is valid, otherwise InternalError * @note if successful, it is ensure that intersection_lanelets_, * intersection_lanelets.first_conflicting_lane are not null * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( + intersection::Result prepareIntersectionData( const bool is_prioritized, PathWithLaneId * path); /** @@ -518,16 +539,6 @@ class IntersectionModule : public SceneModuleInterface const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); /** @} */ -private: - /** - * @defgroup utility [fn] utility member function - * @{ - */ - void stoppedAtPositionForDuration( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t position, - const double duration, StateMachine * state_machine); - /** @} */ - private: /** *********************************************************** @@ -547,14 +558,6 @@ class IntersectionModule : public SceneModuleInterface TrafficPrioritizedLevel getTrafficPrioritizedLevel() const; /** @} */ -private: - /** - * @brief categorize target objects - */ - TargetObjects generateTargetObjects( - const intersection::IntersectionLanelets & intersection_lanelets, - const std::optional & intersection_area) const; - private: /** *********************************************************** @@ -598,14 +601,12 @@ class IntersectionModule : public SceneModuleInterface std::optional isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const; + const intersection::IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -621,27 +622,21 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check occlusion status * @attention this function has access to value() of occlusion_attention_divisions_, - * intersection_lanelets.first_attention_area() + * intersection_lanelets_ intersection_lanelets.first_attention_area() */ std::tuple< OcclusionType, bool /* module detection with margin */, bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects); + const intersection::InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) + * @attention this function has access to value() of intersection_lanelets_, + * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects); + OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info); /** @} */ private: @@ -654,15 +649,12 @@ class IntersectionModule : public SceneModuleInterface */ /** * @brief check if ego is already over the pass judge line - * @return if ego is over both 1st/2nd pass judge lines, return Indecisive, else return + * @return if ego is over both 1st/2nd pass judge lines, return InternalError, else return * (is_over_1st_pass_judge, is_over_2nd_pass_judge) * @attention this function has access to value() of intersection_stoplines.default_stopline, * intersection_stoplines.occlusion_stopline */ - intersection::Result< - intersection::Indecisive, - std::pair> - isOverPassJudgeLinesStatus( + PassJudgeStatus isOverPassJudgeLinesStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -678,6 +670,25 @@ class IntersectionModule : public SceneModuleInterface bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + /** + * @brief find the objects on attention_area/intersection_area and update positional information + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerArea(); + + /** + * @brief find the collision Interval/CollisionKnowledge of registered objects + * @attention this function has access to value() of intersection_lanelets_ + */ + void updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time); + + void cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + /** * @brief check if there are any objects around the stoplines on the attention areas when ego * entered the intersection on green light @@ -687,44 +698,58 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_peeking_stopline */ std::optional isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects); + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines); + + /** + * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and + * blame past perception fault + */ + std::string generateDetectionBlameDiagnosis( + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; /** - * @brief check collision + * @brief generate the message explaining how much ego should accelerate to avoid future dangerous + * situation */ - bool checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const intersection::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level); + std::string generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const TimeDistanceArray & ego_time_distance_array, + const std::vector< + std::pair>> & + too_late_detect_objects, + const std::vector< + std::pair>> & + misjudge_objects) const; + + /** + * @brief return if collision is detected and the collision position + */ + CollisionStatus detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line); std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, const bool is_parked_vehicle) const; - void cutPredictPathWithDuration(TargetObjects * target_objects, const double time_thr); - /** * @brief calculate ego vehicle profile along the path inside the intersection as the sequence of * (time of arrival, traveled distance) from current ego position + * @attention this function has access to value() of + * intersection_stoplines.occlusion_peeking_stopline, + * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); /** @} */ - /* - bool IntersectionModule::checkFrontVehicleDeceleration( - lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, - const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double assumed_front_car_decel); - */ - mutable DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; rclcpp::Publisher::SharedPtr ego_ttc_pub_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 43bb68cf56f67..4f7e8b45d107f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -18,42 +18,18 @@ #include // for toGeomPoly #include // for smoothPath #include +#include #include #include // for toPolygon2d #include -#include #include +#include +#include +#include #include -namespace -{ -tier4_autoware_utils::Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) -{ - namespace bg = boost::geometry; - const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); - const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); - - tier4_autoware_utils::Polygon2d one_step_poly; - for (const auto & point : prev_poly.outer()) { - one_step_poly.outer().push_back(point); - } - for (const auto & point : next_poly.outer()) { - one_step_poly.outer().push_back(point); - } - - bg::correct(one_step_poly); - - tier4_autoware_utils::Polygon2d convex_one_step_poly; - bg::convex_hull(one_step_poly, convex_one_step_poly); - - return convex_one_step_poly; -} -} // namespace - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -79,38 +55,303 @@ bool IntersectionModule::isTargetCollisionVehicleType( return false; } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) +void IntersectionModule::updateObjectInfoManagerArea() { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); - // If there are any vehicles on the attention area when ego entered the intersection on green - // light, do pseudo collision detection because the vehicles are very slow and no collisions may - // be detected. check if ego vehicle entered assigned lanelet - const bool is_green_solid_on = isGreenSolidOn(); - if (!is_green_solid_on) { - return std::nullopt; + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); + + // ========================================================================================== + // entries that are not observed in this iteration need to be cleared + // + // NOTE: old_map is not referenced because internal data of object_info_manager is cleared + // ========================================================================================== + const auto old_map = object_info_manager_.getObjectsMap(); + object_info_manager_.clearObjects(); + + for (const auto & predicted_object : planner_data_->predicted_objects->objects) { + if (!isTargetCollisionVehicleType(predicted_object)) { + continue; + } + + // ========================================================================================== + // NOTE: is_parked_vehicle is used because sometimes slow vehicle direction is + // incorrect/reversed/flipped due to tracking. if is_parked_vehicle is true, object direction + // is not checked + // ========================================================================================== + const auto object_direction = + util::getObjectPoseWithVelocityDirection(predicted_object.kinematics); + const auto is_parked_vehicle = + std::fabs(predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + + const auto belong_adjacent_lanelet_id = + checkAngleForTargetLanelets(object_direction, adjacent_lanelets, false); + if (belong_adjacent_lanelet_id) { + continue; + } + const auto belong_attention_lanelet_id = + checkAngleForTargetLanelets(object_direction, attention_lanelets, is_parked_vehicle); + const auto & obj_pos = predicted_object.kinematics.initial_pose_with_covariance.pose.position; + const bool in_intersection_area = [&]() { + if (!intersection_area) { + return false; + } + return bg::within( + tier4_autoware_utils::Point2d{obj_pos.x, obj_pos.y}, intersection_area.value()); + }(); + std::optional attention_lanelet{std::nullopt}; + std::optional stopline{std::nullopt}; + if (!belong_attention_lanelet_id && !in_intersection_area) { + continue; + } else if (belong_attention_lanelet_id) { + const auto idx = belong_attention_lanelet_id.value(); + attention_lanelet = attention_lanelets.at(idx); + stopline = attention_lanelet_stoplines.at(idx); + } + + const auto object_it = old_map.find(predicted_object.object_id); + if (object_it != old_map.end()) { + auto object_info = object_it->second; + object_info_manager_.registerExistingObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle, object_info); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } else { + auto object_info = object_info_manager_.registerObject( + predicted_object.object_id, belong_attention_lanelet_id.has_value(), in_intersection_area, + is_parked_vehicle); + object_info->initialize(predicted_object, attention_lanelet, stopline); + } } - const auto closest_idx = intersection_stoplines.closest_idx; - const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - if (!initial_green_light_observed_time_) { - const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); - const bool approached_assigned_lane = - motion_utils::calcSignedArcLength( - path.points, closest_idx, - tier4_autoware_utils::createPoint( - assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), - assigned_lane_begin_point.z())) < - planner_param_.collision_detection.yield_on_green_traffic_light - .distance_to_assigned_lanelet_start; - if (approached_assigned_lane) { - initial_green_light_observed_time_ = clock_->now(); +} + +void IntersectionModule::updateObjectInfoManagerCollision( + const intersection::PathLanelets & path_lanelets, + const IntersectionModule::TimeDistanceArray & time_distance_array, + const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time) +{ + const auto & intersection_lanelets = intersection_lanelets_.value(); + + if (passed_1st_judge_line_first_time) { + object_info_manager_.setPassed1stPassJudgeLineFirstTime(clock_->now()); + } + if (passed_2nd_judge_line_first_time) { + object_info_manager_.setPassed2ndPassJudgeLineFirstTime(clock_->now()); + } + + const double passing_time = time_distance_array.back().first; + const auto & concat_lanelets = path_lanelets.all; + const auto closest_arc_coords = + lanelet::utils::getArcCoordinates(concat_lanelets, planner_data_->current_odometry->pose); + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + debug_data_.ego_lane = ego_lane.polygon3d(); + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + // ========================================================================================== + // dynamically change TTC margin according to traffic light color to gradually relax from green to + // red + // ========================================================================================== + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); + + for (auto & object_info : object_info_manager_.attentionObjects()) { + const auto & predicted_object = object_info->predicted_object(); + bool safe_under_traffic_control = false; + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + object_info->can_stop_before_stopline( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)) { + safe_under_traffic_control = true; + } + if ( + traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && + object_info->can_stop_before_ego_lane( + planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration, + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path, + ego_lane)) { + safe_under_traffic_control = true; + } + + // ========================================================================================== + // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe + // CollisionKnowledge for most probable path + // ========================================================================================== + std::list sorted_predicted_paths; + for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { + sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); + } + sorted_predicted_paths.sort( + [](const auto path1, const auto path2) { return path1->confidence > path2->confidence; }); + + // ========================================================================================== + // if all of the predicted path is lower confidence/geometrically does not intersect with ego + // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either + // of them has value, not both + // ========================================================================================== + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; + + for (const auto & predicted_path_ptr : sorted_predicted_paths) { + auto predicted_path = *predicted_path_ptr; + if ( + predicted_path.confidence < + planner_param_.collision_detection.min_predicted_path_confidence) { + continue; + } + cutPredictPathWithinDuration( + planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); + const auto object_passage_interval_opt = intersection::findPassageInterval( + predicted_path, predicted_object.shape, ego_poly, + intersection_lanelets.first_attention_lane(), + intersection_lanelets.second_attention_lane()); + if (!object_passage_interval_opt) { + // there is no chance of geometric collision for the entire prediction horizon + continue; + } + const auto & object_passage_interval = object_passage_interval_opt.value(); + const auto [object_enter_time, object_exit_time] = object_passage_interval.interval_time; + const auto ego_start_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_enter_time - collision_start_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_start_itr == time_distance_array.end()) { + // ========================================================================================== + // this is the case where at time "object_enter_time - collision_start_margin_time", ego is + // arriving at the exit of the intersection, which means even if we assume that the object + // accelerates and the first collision happens faster by the TTC margin, ego will be already + // arriving at the exist of the intersection. + // ========================================================================================== + continue; + } + auto ego_end_itr = std::lower_bound( + time_distance_array.begin(), time_distance_array.end(), + object_exit_time + collision_end_margin_time, + [](const auto & a, const double b) { return a.first < b; }); + if (ego_end_itr == time_distance_array.end()) { + ego_end_itr = time_distance_array.end() - 1; + } + const double ego_start_arc_length = std::max( + 0.0, closest_arc_coords.length + ego_start_itr->second - + planner_data_->vehicle_info_.rear_overhang_m); + const double ego_end_arc_length = std::min( + closest_arc_coords.length + ego_end_itr->second + + planner_data_->vehicle_info_.max_longitudinal_offset_m, + lanelet::utils::getLaneletLength2d(concat_lanelets)); + const auto trimmed_ego_polygon = lanelet::utils::getPolygonFromArcLength( + concat_lanelets, ego_start_arc_length, ego_end_arc_length); + if (trimmed_ego_polygon.empty()) { + continue; + } + Polygon2d polygon{}; + for (const auto & p : trimmed_ego_polygon) { + polygon.outer().emplace_back(p.x(), p.y()); + } + bg::correct(polygon); + debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); + + const auto & object_path = object_passage_interval.path; + const auto [begin, end] = object_passage_interval.interval_position; + bool collision_detected = false; + for (auto i = begin; i <= end; ++i) { + if (bg::intersects( + polygon, + tier4_autoware_utils::toPolygon2d(object_path.at(i), predicted_object.shape))) { + collision_detected = true; + break; + } + } + if (collision_detected) { + // if judged as UNSAFE, return + safe_interval = std::nullopt; + unsafe_interval = object_passage_interval; + break; + } + if (!safe_interval) { + // ========================================================================================== + // save the safe_decision_knowledge for the most probable path. this value is nullified if + // judged UNSAFE during the iteration + // ========================================================================================== + safe_interval = object_passage_interval; + } + } + object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); + if (passed_1st_judge_line_first_time) { + object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + if (passed_2nd_judge_line_first_time) { + object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + clock_->now(), // stamp + unsafe_interval + ? intersection::CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control + ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : intersection::CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval + predicted_object.kinematics.initial_twist_with_covariance.twist.linear + .x // observed_velocity + }); + } + } +} + +void IntersectionModule::cutPredictPathWithinDuration( + const builtin_interfaces::msg::Time & object_stamp, const double time_thr, + autoware_auto_perception_msgs::msg::PredictedPath * path) const +{ + const rclcpp::Time current_time = clock_->now(); + const auto original_path = path->path; + path->path.clear(); + + for (size_t k = 0; k < original_path.size(); ++k) { // each path points + const auto & predicted_pose = original_path.at(k); + const auto predicted_time = + rclcpp::Time(object_stamp) + rclcpp::Duration(path->time_step) * static_cast(k); + if ((predicted_time - current_time).seconds() < time_thr) { + path->path.push_back(predicted_pose); } } +} + +std::optional +IntersectionModule::isGreenPseudoCollisionStatus( + const size_t closest_idx, const size_t collision_stopline_idx, + const intersection::IntersectionStopLines & intersection_stoplines) +{ + // ========================================================================================== + // if there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because collision is likely to happen. + // ========================================================================================== if (initial_green_light_observed_time_) { const auto now = clock_->now(); const bool still_wait = @@ -119,22 +360,341 @@ IntersectionModule::isGreenPseudoCollisionStatus( if (!still_wait) { return std::nullopt; } + const auto & attention_objects = object_info_manager_.attentionObjects(); const bool exist_close_vehicles = std::any_of( - target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), - [&](const auto & object) { - return object.dist_to_stopline.has_value() && - object.dist_to_stopline.value() < - planner_param_.collision_detection.yield_on_green_traffic_light - .object_dist_to_stopline; + attention_objects.begin(), attention_objects.end(), [&](const auto & object_info) { + return object_info->before_stopline_by( + planner_param_.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline); }); if (exist_close_vehicles) { + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } return std::nullopt; } +std::string IntersectionModule::generateDetectionBlameDiagnosis( + const std::vector>> & + too_late_detect_objects, + const std::vector>> & + misjudge_objects) const +{ + std::string diag; + if (!safely_passed_1st_judge_line_time_) { + return diag; + } + const auto [passed_1st_judge_line_time, passed_1st_judge_line_pose] = + safely_passed_1st_judge_line_time_.value(); + const auto passed_1st_judge_line_time_double = + static_cast(passed_1st_judge_line_time.nanoseconds()) / 1e+9; + + const auto now = clock_->now(); + const auto now_double = static_cast(now.nanoseconds()) / 1e+9; + + // CAVEAT: format library causes runtime fault if the # of placeholders is more than the # of + // vargs + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval()) { + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_1st_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 1st pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the first attention lanelet of type " + "{5}.\n", + object_info->uuid_str, // 0 + passed_1st_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 1st pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 1st pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_1st_judge_line_pose.position.x, // 4 + passed_1st_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_1st_judge_line_pose, past_position) // 6 + ); + } + } + if ( + safely_passed_2nd_judge_line_time_ && + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval()) { + const auto [passed_2nd_judge_line_time, passed_2nd_judge_line_pose] = + safely_passed_2nd_judge_line_time_.value(); + const auto passed_2nd_judge_line_time_double = + static_cast(passed_2nd_judge_line_time.nanoseconds()) / 1e+9; + + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const double time_diff = now_double - passed_2nd_judge_line_time_double; + diag += fmt::format( + "object {0} was not detected when ego passed the 2nd pass judge line at {1}, but now at " + "{2}, collision is detected after {3}~{4} seconds on the lanelet of type {5}.\n", + object_info->uuid_str, // 0 + passed_2nd_judge_line_time_double, // 1 + now_double, // 2 + unsafe_interval.interval_time.first, // 3 + unsafe_interval.interval_time.second, // 4 + magic_enum::enum_name(unsafe_interval.lane_position) // 5 + ); + const auto past_position_opt = object_info->estimated_past_position(time_diff); + if (past_position_opt) { + const auto & past_position = past_position_opt.value(); + diag += fmt::format( + "this object is estimated to have been at x = {0}, y = {1} when ego passed the 2nd pass " + "judge line({2} seconds before from now) given the estimated current velocity {3}[m/s]. " + "ego was at x = {4}, y = {5} when it passed the 2nd pass judge line so it is the fault " + "of detection side that failed to detect around {6}[m] range at that time.\n", + past_position.x, // 0 + past_position.y, // 1 + time_diff, // 2 + object_info->observed_velocity(), // 3 + passed_2nd_judge_line_pose.position.x, // 4 + passed_2nd_judge_line_pose.position.y, // 5 + tier4_autoware_utils::calcDistance2d(passed_2nd_judge_line_pose, past_position) // 6 + ); + } + } + } + for (const auto & [blame_type, object_info] : misjudge_objects) { + if ( + blame_type == CollisionStatus::BLAME_AT_FIRST_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_1st_pass_judge_line_passage()) { + const auto & decision_at_1st_pass_judge_line = + object_info->decision_at_1st_pass_judge_line_passage().value(); + const auto decision_at_1st_pass_judge_line_time = + static_cast(decision_at_1st_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 1st pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the first attention lanelet of type {7} with the estimated " + "current velocity {8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_1st_pass_judge_line.safe_type), // 1 + decision_at_1st_pass_judge_line_time, // 2 + decision_at_1st_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + if ( + blame_type == CollisionStatus::BLAME_AT_SECOND_PASS_JUDGE && object_info->unsafe_interval() && + object_info->decision_at_2nd_pass_judge_line_passage()) { + const auto & decision_at_2nd_pass_judge_line = + object_info->decision_at_2nd_pass_judge_line_passage().value(); + const auto decision_at_2nd_pass_judge_line_time = + static_cast(decision_at_2nd_pass_judge_line.stamp.nanoseconds()) / 1e+9; + const auto & unsafe_interval = object_info->unsafe_interval().value(); + diag += fmt::format( + "object {0} was judged as {1} when ego passed the 2nd pass judge line at time {2} " + "previously with the estimated velocity {3}[m/s], but now at {4} collision is detected " + "after {5}~{6} seconds on the lanelet of type {7} with the estimated current velocity " + "{8}[m/s]\n", + object_info->uuid_str, // 0 + magic_enum::enum_name(decision_at_2nd_pass_judge_line.safe_type), // 1 + decision_at_2nd_pass_judge_line_time, // 2 + decision_at_2nd_pass_judge_line.observed_velocity, // 3 + now_double, // 4 + unsafe_interval.interval_time.first, // 5 + unsafe_interval.interval_time.second, // 6 + magic_enum::enum_name(unsafe_interval.lane_position), // 7 + object_info->observed_velocity() // 8 + ); + } + } + return diag; +} + +std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const IntersectionModule::TimeDistanceArray & ego_time_distance_array, + const std::vector>> & + too_late_detect_objects, + [[maybe_unused]] const std::vector>> & + misjudge_objects) const +{ + static constexpr double min_vel = 1e-2; + std::string diag; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + for (const auto & [blame_type, object_info] : too_late_detect_objects) { + if (!object_info->unsafe_interval()) { + continue; + } + // object side + const auto & unsafe_interval = object_info->unsafe_interval().value(); + const auto [begin, end] = unsafe_interval.interval_position; + const auto &p1 = unsafe_interval.path.at(begin).position, + p2 = unsafe_interval.path.at(end).position; + const auto collision_pos = + tier4_autoware_utils::createPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2, (p1.z + p2.z) / 2); + const auto object_dist_to_margin_point = + tier4_autoware_utils::calcDistance2d( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose.position, + collision_pos) - + planner_param_.collision_detection.avoid_collision_by_acceleration + .object_time_margin_to_collision_point * + object_info->observed_velocity(); + if (object_dist_to_margin_point <= 0.0) { + continue; + } + const auto object_eta_to_margin_point = + object_dist_to_margin_point / std::max(min_vel, object_info->observed_velocity()); + // ego side + const double ego_dist_to_collision_pos = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_pos); + const auto ego_eta_to_collision_pos_it = std::lower_bound( + ego_time_distance_array.begin(), ego_time_distance_array.end(), ego_dist_to_collision_pos, + [](const auto & a, const double b) { return a.second < b; }); + if (ego_eta_to_collision_pos_it == ego_time_distance_array.end()) { + continue; + } + const double ego_eta_to_collision_pos = ego_eta_to_collision_pos_it->first; + if (ego_eta_to_collision_pos < object_eta_to_margin_point) { + // this case, ego will pass the collision point before this object get close to the collision + // point within margin just by keeping current plan, so no need to accelerate + continue; + } + diag += fmt::format( + "the object is expected to approach the collision point by the TTC margin in {0} seconds, " + "while ego will arrive there in {1} seconds, so ego needs to accelerate from current " + "velocity {2}[m/s] to {3}[m/s]\n", + object_eta_to_margin_point, // 0 + ego_eta_to_collision_pos, // 1 + ego_vel, // 2 + ego_dist_to_collision_pos / object_eta_to_margin_point // 3 + ); + } + return diag; +} + +IntersectionModule::CollisionStatus IntersectionModule::detectCollision( + const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) +{ + // ========================================================================================== + // if collision is detected for multiple objects, we prioritize collision on the first + // attention lanelet + // ========================================================================================== + bool collision_at_first_lane = false; + bool collision_at_non_first_lane = false; + + // ========================================================================================== + // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // + // misjudge_objects are those that were once judged as safe when ego passed the pass judge line + // + // too_late_detect objects are those that (1) were not detected when ego passed the pass judge + // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are + // expected to have been detected in the prior iteration because ego could have judged as UNSAFE + // in the prior iteration + // ========================================================================================== + std::vector>> + misjudge_objects; + std::vector>> + too_late_detect_objects; + for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (object_info->is_safe_under_traffic_control()) { + debug_data_.safe_under_traffic_control_targets.objects.push_back( + object_info->predicted_object()); + continue; + } + if (!object_info->is_unsafe()) { + continue; + } + const auto & unsafe_info = object_info->is_unsafe().value(); + // ========================================================================================== + // if ego is over the pass judge lines, then the visualization as "too_late_objects" or + // "misjudge_objects" is more important than that for "unsafe" + // + // NOTE: consider a vehicle which was not detected at 1st_pass_judge_passage, and now collision + // detected on the 1st lane, which is "too_late" for 1st lane passage, but once it decelerated + // or yielded, so it turned safe, and ego passed the 2nd pass judge line, but at the same it + // accelerated again, which is "misjudge" for 2nd lane passage. In this case this vehicle is + // visualized as "misjudge" + // ========================================================================================== + auto * debug_container = &debug_data_.unsafe_targets.objects; + if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + collision_at_first_lane = true; + } else { + collision_at_non_first_lane = true; + } + if ( + is_over_1st_pass_judge_line && + unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + const auto & decision_at_1st_pass_judge_opt = + object_info->decision_at_1st_pass_judge_line_passage(); + if (!decision_at_1st_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); + if ( + decision_at_1st_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + if (is_over_2nd_pass_judge_line && is_over_2nd_pass_judge_line.value()) { + const auto & decision_at_2nd_pass_judge_opt = + object_info->decision_at_2nd_pass_judge_line_passage(); + if (!decision_at_2nd_pass_judge_opt) { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } else { + const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); + if ( + decision_at_2nd_pass_judge.safe_type != + intersection::CollisionKnowledge::SafeType::UNSAFE) { + misjudge_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.misjudge_targets.objects; + } else { + too_late_detect_objects.emplace_back( + CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); + debug_container = &debug_data_.too_late_detect_targets.objects; + } + } + } + debug_container->emplace_back(object_info->predicted_object()); + } + if (collision_at_first_lane) { + return { + true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + } else if (collision_at_non_first_lane) { + return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + } + return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; +} + +/* bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, const intersection::PathLanelets & path_lanelets, const size_t closest_idx, @@ -383,6 +943,7 @@ bool IntersectionModule::checkCollision( return collision_detected; } +*/ std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -422,32 +983,9 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( return std::nullopt; } -void IntersectionModule::cutPredictPathWithDuration( - TargetObjects * target_objects, const double time_thr) -{ - const rclcpp::Time current_time = clock_->now(); - for (auto & target_object : target_objects->all_attention_objects) { // each objects - for (auto & predicted_path : - target_object.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(target_objects->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); - } - } - } - } -} - IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { const double intersection_velocity = @@ -460,8 +998,42 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity; const double current_velocity = planner_data_->current_velocity->twist.linear.x; - int assigned_lane_found = false; + // ========================================================================================== + // if ego is waiting for collision detection, the entry time into the intersection + // is a bit delayed for the chattering hold, so we need to "shift" the TimeDistanceArray by + // this delay + // ========================================================================================== + const bool is_go_out = (activated_ && occlusion_activated_); + const double time_delay = (is_go_out || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); + // ========================================================================================== + // to account for the stopline generated by upstream behavior_velocity modules (like walkway, + // crosswalk), if use_upstream flag is true, the raw velocity of path points after + // last_intersection_stopline_candidate_idx is used, which maybe almost-zero. at those almost-zero + // velocity path points, ego future profile is almost "fixed" there. + // + // last_intersection_stopline_candidate_idx must be carefully decided especially when ego + // velocity is almost zero, because if last_intersection_stopline_candidate_idx is at the + // closest_idx for example, ego is almost "fixed" at current position for the entire + // spatiotemporal profile, which is judged as SAFE because that profile does not collide + // with the predicted paths of objects. + // + // if second_attention_lane exists, second_attention_stopline_idx is used. if not, + // max(occlusion_stopline_idx, first_attention_stopline_idx) is used because + // occlusion_stopline_idx varies depending on the peeking offset parameter + // ========================================================================================== + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); + const auto closest_idx = intersection_stoplines.closest_idx; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() + : std::max(occlusion_stopline_idx, first_attention_stopline_idx); + + int assigned_lane_found = false; // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 353826070eff7..b7c2d4c12878c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -34,25 +34,18 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionLanelets & intersection_lanelets, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { - const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); - const auto first_attention_area = intersection_lanelets.first_attention_area().value(); const bool is_amber_or_red = (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED); // check occlusion on detection lane - const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); auto occlusion_status = (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) - ? detectOcclusion( - occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, - occlusion_attention_divisions, target_objects) + ? detectOcclusion(interpolated_path_info) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -76,13 +69,14 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const std::vector & attention_areas, - const lanelet::ConstLanelets & adjacent_lanelets, - const lanelet::CompoundPolygon3d & first_attention_area, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const TargetObjects & target_objects) + const intersection::InterpolatedPathInfo & interpolated_path_info) { + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & attention_areas = intersection_lanelets.occlusion_attention_area(); + const auto first_attention_area = intersection_lanelets.first_attention_area().value(); + const auto & lane_divisions = occlusion_attention_divisions_.value(); + const auto & occ_grid = *planner_data_->occupancy_grid; const auto & current_pose = planner_data_->current_odometry->pose; const double occlusion_dist_thr = planner_param_.occlusion.occlusion_required_clearance_distance; @@ -208,13 +202,15 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded - const auto & blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + const auto & blocking_attention_objects = object_info_manager_.parkedObjects(); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + debug_data_.parked_targets.objects.push_back( + blocking_attention_object_info->predicted_object()); } std::vector> blocking_polygons; - for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); + for (const auto & blocking_attention_object_info : blocking_attention_objects) { + const Polygon2d obj_poly = + tier4_autoware_utils::toPolygon2d(blocking_attention_object_info->predicted_object()); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { @@ -390,14 +386,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( LineString2d ego_occlusion_line; ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); - for (const auto & attention_object : target_objects.all_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); - if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; - } - } - for (const auto & attention_object : target_objects.intersection_area_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + for (const auto & attention_object_info : object_info_manager_.allObjects()) { + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_line)) { return OcclusionType::DYNAMICALLY_OCCLUDED; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 746e615d8c6b0..c44008db9b08b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -161,11 +161,13 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -intersection::Result +using intersection::make_err; +using intersection::make_ok; +using intersection::Result; + +Result IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithLaneId * path) { - using intersection::Result; - const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); const auto & assigned_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id_); @@ -177,26 +179,32 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - return Result::make_err( - intersection::Indecisive{"splineInterpolate failed"}); + return make_err( + "splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return Result::make_err( - intersection::Indecisive{ - "Path has no interval on intersection lane " + std::to_string(lane_id_)}); + return make_err( + "Path has no interval on intersection lane " + std::to_string(lane_id_)); } - // cache intersection lane information because it is invariant if (!intersection_lanelets_) { intersection_lanelets_ = generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); } auto & intersection_lanelets = intersection_lanelets_.value(); - - // at the very first time of regisTration of this module, the path may not be conflicting with the - // attention area, so update() is called to update the internal data as well as traffic light info + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.first_attention_area = intersection_lanelets.first_attention_area(); + debug_data_.second_attention_area = intersection_lanelets.second_attention_area(); + debug_data_.occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); + + // ========================================================================================== + // at the very first time of registration of this module, the path may not be conflicting with + // the attention area, so update() is called to update the internal data as well as traffic + // light info + // ========================================================================================== intersection_lanelets.update( is_prioritized, interpolated_path_info, footprint, baselink2front, routing_graph_ptr); @@ -205,17 +213,17 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return Result::make_err( - intersection::Indecisive{"conflicting area is empty"}); + return make_err( + "conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); const auto & second_attention_area_opt = intersection_lanelets.second_attention_area(); - // generate all stop line candidates - // see the doc for struct IntersectionStopLines - /// even if the attention area is null, stuck vehicle stop line needs to be generated from - /// conflicting lanes + // ========================================================================================== + // even if the attention area is null, stuck vehicle stop line needs to be generated from + // conflicting lanes, so dummy_first_attention_lane is used + // ========================================================================================== const auto & dummy_first_attention_lane = intersection_lanelets.first_attention_lane() ? intersection_lanelets.first_attention_lane().value() : first_conflicting_lane; @@ -224,8 +232,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return Result::make_err( - intersection::Indecisive{"failed to generate intersection_stoplines"}); + return make_err( + "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); const auto closest_idx = intersection_stoplines.closest_idx; @@ -239,8 +247,8 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return Result::make_err( - intersection::Indecisive{"failed to generate PathLanelets"}); + return make_err( + "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -250,8 +258,24 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL planner_data_->occupancy_grid->info.resolution); } - return Result::make_ok( - BasicData{interpolated_path_info, intersection_stoplines, path_lanelets}); + const bool is_green_solid_on = isGreenSolidOn(); + if (is_green_solid_on && !initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + + return make_ok( + interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( @@ -421,8 +445,10 @@ IntersectionModule::generateIntersectionStopLines( int stuck_stopline_ip_int = 0; bool stuck_stopline_valid = true; if (use_stuck_stopline) { + // ========================================================================================== // NOTE: when ego vehicle is approaching attention area and already passed // first_conflicting_area, this could be null. + // ========================================================================================== const auto stuck_stopline_idx_ip_opt = util::getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); if (!stuck_stopline_idx_ip_opt) { @@ -457,12 +483,13 @@ IntersectionModule::generateIntersectionStopLines( second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge line position on interpolated path. It is the same as first pass judge - // line if second_attention_lane is null - int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; - const auto second_pass_judge_line_ip = - second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) - : first_pass_judge_line_ip; + // (8) second pass judge line position on interpolated path. It is null if second_attention_lane + // is null + size_t second_pass_judge_line_ip = occlusion_wo_tl_pass_judge_line_ip; + bool second_pass_judge_line_valid = false; + if (second_attention_area_opt) { + second_pass_judge_line_valid = true; + } struct IntersectionStopLinesTemp { @@ -528,9 +555,11 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines.occlusion_peeking_stopline = intersection_stoplines_temp.occlusion_peeking_stopline; } + if (second_pass_judge_line_valid) { + intersection_stoplines.second_pass_judge_line = + intersection_stoplines_temp.second_pass_judge_line; + } intersection_stoplines.first_pass_judge_line = intersection_stoplines_temp.first_pass_judge_line; - intersection_stoplines.second_pass_judge_line = - intersection_stoplines_temp.second_pass_judge_line; intersection_stoplines.occlusion_wo_tl_pass_judge_line = intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; return intersection_stoplines; @@ -556,7 +585,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets } // for low priority lane - // If ego_lane has right of way (i.e. is high priority), + // if ego_lane has right of way (i.e. is high priority), // ignore yieldLanelets (i.e. low priority lanes) lanelet::ConstLanelets yield_lanelets{}; const auto right_of_ways = assigned_lanelet.regulatoryElementsAs(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 7f23bed3c36cd..389c73d651f1e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -262,21 +262,19 @@ bool IntersectionModule::checkStuckVehicleInIntersection( std::optional IntersectionModule::isYieldStuckStatus( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines, - const TargetObjects & target_objects) const + const intersection::IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { return motion_utils::calcSignedArcLength(path.points, closest_idx, index); }; - const auto & intersection_lanelets = intersection_lanelets_.value(); // this is OK - const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); // this is OK - const auto first_attention_stopline_idx = - intersection_stoplines.first_attention_stopline.value(); // this is OK + const auto & intersection_lanelets = intersection_lanelets_.value(); + const auto default_stopline_idx = intersection_stoplines.default_stopline.value(); + const auto first_attention_stopline_idx = intersection_stoplines.first_attention_stopline.value(); const auto stuck_stopline_idx_opt = intersection_stoplines.stuck_stopline; const bool yield_stuck_detected = checkYieldStuckVehicleInIntersection( - target_objects, interpolated_path_info, intersection_lanelets.attention_non_preceding()); + interpolated_path_info, intersection_lanelets.attention_non_preceding()); if (yield_stuck_detected) { std::optional stopline_idx = std::nullopt; const bool is_before_default_stopline = fromEgoDist(default_stopline_idx) >= 0.0; @@ -297,14 +295,13 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value()}; + return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const TargetObjects & target_objects, const intersection::InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { @@ -358,19 +355,20 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( ::createLaneletFromArcLength(attention_lanelet, yield_stuck_start, yield_stuck_end)); } debug_data_.yield_stuck_detect_area = util::getPolygon3dFromLanelets(yield_stuck_detect_lanelets); - for (const auto & object : target_objects.all_attention_objects) { + for (const auto & object_info : object_info_manager_.attentionObjects()) { + const auto & object = object_info->predicted_object(); const auto obj_v_norm = std::hypot( - object.object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.object.kinematics.initial_twist_with_covariance.twist.linear.y); + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); if (obj_v_norm > stuck_vehicle_vel_thr) { continue; } for (const auto & yield_stuck_detect_lanelet : yield_stuck_detect_lanelets) { const bool is_in_lanelet = lanelet::utils::isInLanelet( - object.object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); + object.kinematics.initial_pose_with_covariance.pose, yield_stuck_detect_lanelet); if (is_in_lanelet) { - debug_data_.yield_stuck_targets.objects.push_back(object.object); + debug_data_.yield_stuck_targets.objects.push_back(object); return true; } } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f103191b2dc6f..e37bb3ee88cc1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -34,7 +34,6 @@ namespace behavior_velocity_planner::util { /** - * @fn * @brief insert a new pose to the path and return its index * @return if insertion was successful return the inserted point index */ @@ -44,7 +43,6 @@ std::optional insertPointIndex( const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** - * @fn * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( @@ -52,7 +50,6 @@ bool hasLaneIds( const std::set & ids); /** - * @fn * @brief find the first contiguous interval of the path points that contains the specified lane ids * @return if no interval is found, return null. if the interval [start, end] (inclusive range) is * found, returns the pair (start-1, end) @@ -61,7 +58,6 @@ std::optional> findLaneIdsInterval( const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** - * @fn * @brief return the index of the first point which is inside the given polygon * @param[in] lane_interval the interval of the path points on the intersection * @param[in] search_forward flag for search direction @@ -72,7 +68,6 @@ std::optional getFirstPointInsidePolygon( const bool search_forward = true); /** - * @fn * @brief check if ego is over the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -84,7 +79,6 @@ bool isOverTargetIndex( const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** - * @fn * @brief check if ego is before the target_idx. If the index is same, compare the exact pose * @param[in] path path * @param[in] closest_idx ego's closest index on the path @@ -99,13 +93,11 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); /** - * @fn * @brief check if the given lane has related traffic light */ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); /** - * @fn * @brief interpolate PathWithLaneId */ std::optional generateInterpolatedPath( @@ -117,7 +109,6 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** - * @fn * @brief this function sorts the set of lanelets topologically using topological sort and merges * the lanelets from each root to each end. each branch is merged and returned with the original * lanelets @@ -131,7 +122,6 @@ mergeLaneletsByTopologicalSort( const lanelet::routing::RoutingGraphPtr routing_graph_ptr); /** - * @fn * @brief find the index of the first point where vehicle footprint intersects with the given * polygon */ @@ -140,6 +130,10 @@ std::optional getFirstPointInsidePolygonByFootprint( const intersection::InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); +/** + * @brief find the index of the first point where vehicle footprint intersects with the given + * polygons + */ std::optional> getFirstPointInsidePolygonsByFootprint( From 91267c486dd34294dfa0eb30b7e70b979a085155 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 29 Jan 2024 22:15:57 +0900 Subject: [PATCH 22/36] fix(intersection): judge pass judge at intersection without tl with occlusion detection (#6207) Signed-off-by: Mamoru Sobue --- .../src/decision_result.cpp | 14 ++++---- .../src/scene_intersection.cpp | 33 ++++++++++++++++--- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp index 93a7c2a29d2f7..aefd59a72f9b4 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -19,11 +19,11 @@ namespace behavior_velocity_planner::intersection std::string formatDecisionResult(const DecisionResult & decision_result) { if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "InternalError because " + state.error; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + state.evasive_report; } @@ -31,11 +31,11 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "StuckStop"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "YieldStuckStop:\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { @@ -45,18 +45,18 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return "PeekingTowardOcclusion"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OccludedCollisionStop\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; } if (std::holds_alternative(decision_result)) { return "Safe"; } if (std::holds_alternative(decision_result)) { - const auto state = std::get(decision_result); + const auto & state = std::get(decision_result); return "FullyPrioritized\nsafety_report:" + state.safety_report; } return ""; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75e2da034780a..3f6136673a44a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -354,8 +354,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( : false; if (!has_traffic_light_) { if (fromEgoDist(occlusion_wo_tl_pass_judge_line_idx) < 0) { - return intersection::InternalError{ - "already passed maximum peeking line in the absence of traffic light"}; + if (has_collision) { + const auto closest_idx = intersection_stoplines.closest_idx; + const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( + *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light.\n" + + safety_report, + evasive_diag}; + } + return intersection::OverPassJudge{ + "already passed maximum peeking line in the absence of traffic light safely", + "no evasive action required"}; } return intersection::OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, @@ -364,7 +374,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_report}; + safety_diag}; } // ========================================================================================== @@ -1251,7 +1261,22 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat return first_pass_judge_line_idx; }(); - const bool was_safe = std::holds_alternative(prev_decision_result_); + // ========================================================================================== + // at intersection without traffic light, this module ignores occlusion even if occlusion is + // detected for real, so if collision is not detected in that context, that should be interpreted + // as "was_safe" + // ========================================================================================== + const bool was_safe = [&]() { + if (std::holds_alternative(prev_decision_result_)) { + return true; + } + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = + std::get(prev_decision_result_); + return !state.collision_detected; + } + return false; + }(); const bool is_over_1st_pass_judge_line = util::isOverTargetIndex(path, closest_idx, current_pose, pass_judge_line_idx); From 39269a8275aedc414a8a2938183f009687ed9339 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 1 Feb 2024 17:23:20 +0900 Subject: [PATCH 23/36] feat(intersection_occlusion)!: react RTC disapproval and stop even if occlusion detection is OFF (#6279) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 3f6136673a44a..3cf243b7893fc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -735,9 +735,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if ( - !rtc_occlusion_approved && decision_result.occlusion_stopline_idx && - planner_param.occlusion.enable) { + if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { const auto occlusion_stopline_idx = decision_result.occlusion_stopline_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -814,7 +812,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -857,7 +855,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { if (planner_param.occlusion.creep_during_peeking.enable) { const size_t occlusion_peeking_stopline = decision_result.occlusion_stopline_idx; const size_t closest_idx = decision_result.closest_idx; @@ -895,7 +893,7 @@ void reactRTCApprovalByDecisionResult( "PeekingTowardOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx @@ -965,7 +963,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.temporal_stop_before_attention_required ? decision_result.first_attention_stopline_idx : decision_result.occlusion_stopline_idx; @@ -1066,7 +1064,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = @@ -1110,7 +1108,7 @@ void reactRTCApprovalByDecisionResult( path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } - if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + if (!rtc_occlusion_approved) { const auto stopline_idx = decision_result.occlusion_stopline_idx; planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = From 7f463133855193c9e37a26ea10a3113eca668f34 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 2 Feb 2024 16:58:55 +0900 Subject: [PATCH 24/36] fix(merge_from_private): fix stop position calculation (#6286) Signed-off-by: Mamoru Sobue --- .../src/scene_merge_from_private_road.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index ba8b6d86350bc..c5136df0bd52e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -129,7 +129,13 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR if (!first_conflicting_idx_opt) { return false; } - const auto stopline_idx_ip = first_conflicting_idx_opt.value(); + // ========================================================================================== + // first_conflicting_idx is calculated considering baselink2front already, so there is no need + // to subtract baselink2front/ds here + // ========================================================================================== + const auto stopline_idx_ip = static_cast(std::max( + 0, static_cast(first_conflicting_idx_opt.value()) - + static_cast(planner_param_.stopline_margin / planner_param_.path_interpolation_ds))); const auto stopline_idx_opt = util::insertPointIndex( interpolated_path_info.path.points.at(stopline_idx_ip).point.pose, path, From cdeed2b19f89060d82515a9bc4a6233b9023bad2 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 5 Feb 2024 23:21:23 +0900 Subject: [PATCH 25/36] fix(intersection): fix stopline midpoint calculation (#6315) Signed-off-by: Mamoru Sobue --- .../src/object_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index ea5d89fe8052d..fe327704f61c2 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -165,7 +165,7 @@ bool ObjectInfo::can_stop_before_ego_lane( const auto stopline_p1 = stopline.front(); const auto stopline_p2 = stopline.back(); const tier4_autoware_utils::Point2d stopline_mid{ - stopline_p1.x() + stopline_p2.x() / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; + (stopline_p1.x() + stopline_p2.x()) / 2.0, (stopline_p1.y() + stopline_p2.y()) / 2.0}; const auto attention_lane_end = attention_lanelet.centerline().back(); const tier4_autoware_utils::LineString2d attention_lane_later_part( {tier4_autoware_utils::Point2d{stopline_mid.x(), stopline_mid.y()}, From c01ded330e209fa7490470c3fe8a1ce4400bc77b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 7 Feb 2024 17:22:58 +0900 Subject: [PATCH 26/36] chore(intersection): add const to member functions, replace enum with class tag for occlusion (#6299) Signed-off-by: Mamoru Sobue --- .../src/object_manager.cpp | 2 +- .../src/object_manager.hpp | 4 +- .../src/scene_intersection.cpp | 41 ++++++++------ .../src/scene_intersection.hpp | 55 +++++++++++-------- .../src/scene_intersection_collision.cpp | 25 +++++---- .../src/scene_intersection_occlusion.cpp | 19 ++++--- .../src/scene_intersection_prepare_data.cpp | 10 ++-- 7 files changed, 91 insertions(+), 65 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index fe327704f61c2..420031e4df1cf 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -240,7 +240,7 @@ void ObjectInfoManager::clearObjects() parked_objects_.clear(); }; -std::vector> ObjectInfoManager::allObjects() +std::vector> ObjectInfoManager::allObjects() const { std::vector> all_objects = attention_area_objects_; all_objects.insert( diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index e77849570cda8..77e39637523a9 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -109,7 +109,7 @@ class ObjectInfo return predicted_object_; }; - std::optional is_unsafe() const + std::optional unsafe_info() const { if (safe_under_traffic_control_) { return std::nullopt; @@ -246,7 +246,7 @@ class ObjectInfoManager const std::vector> & parkedObjects() const { return parked_objects_; } - std::vector> allObjects(); + std::vector> allObjects() const; const std::unordered_map> & getObjectsMap() diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 3cf243b7893fc..dc9c70a2ebc04 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -51,7 +51,6 @@ IntersectionModule::IntersectionModule( const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), - node_(node), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), @@ -87,10 +86,10 @@ IntersectionModule::IntersectionModule( } decision_state_pub_ = - node_.create_publisher("~/debug/intersection/decision_state", 1); - ego_ttc_pub_ = node_.create_publisher( + node.create_publisher("~/debug/intersection/decision_state", 1); + ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); - object_ttc_pub_ = node_.create_publisher( + object_ttc_pub_ = node.create_publisher( "~/debug/intersection/object_ttc", 1); } @@ -104,11 +103,13 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const auto decision_result = modifyPathVelocityDetail(path, stop_reason); prev_decision_result_ = decision_result; - const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + - intersection::formatDecisionResult(decision_result); - std_msgs::msg::String decision_result_msg; - decision_result_msg.data = decision_type; - decision_state_pub_->publish(decision_result_msg); + { + const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + + intersection::formatDecisionResult(decision_result); + std_msgs::msg::String decision_result_msg; + decision_result_msg.data = decision_type; + decision_state_pub_->publish(decision_result_msg); + } prepareRTCStatus(decision_result, *path); @@ -224,6 +225,14 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( updateObjectInfoManagerCollision( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, safely_passed_2nd_judge_line); + for (const auto & object_info : object_info_manager_.attentionObjects()) { + if (!object_info->unsafe_info()) { + continue; + } + setObjectsOfInterestData( + object_info->predicted_object().kinematics.initial_pose_with_covariance.pose, + object_info->predicted_object().shape, ColorName::RED); + } const auto [has_collision, collision_position, too_late_detect_objects, misjudge_objects] = detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); @@ -240,17 +249,17 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( "no collision is detected", "ego can safely pass the intersection at this rate"}; } - const bool collision_on_1st_attention_lane = - has_collision && (collision_position == intersection::CollisionInterval::LanePosition::FIRST); // ========================================================================================== // this state is very dangerous because ego is very close/over the boundary of 1st attention lane // and collision is detected on the 1st lane. Since the 2nd attention lane also exists in this // case, possible another collision may be expected on the 2nd attention lane too. // ========================================================================================== std::string safety_report = safety_diag; - if ( - is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line && - is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { + if (const bool collision_on_1st_attention_lane = + has_collision && + (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() && + !is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { safety_report += "\nego is between the 1st and 2nd pass judge line but collision is expected on the 1st " "attention lane, which is dangerous."; @@ -374,7 +383,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_diag}; + safety_report}; } // ========================================================================================== @@ -382,7 +391,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // // if ego is stuck by static occlusion in the presence of traffic light, start timeout count // ========================================================================================== - const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; + const bool is_static_occlusion = std::holds_alternative(occlusion_status); const bool is_stuck_by_static_occlusion = stoppedAtPosition( occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 9527ce8e5ebea..e62a58d63f923 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -41,6 +41,7 @@ #include #include #include +#include #include namespace behavior_velocity_planner @@ -166,16 +167,25 @@ class IntersectionModule : public SceneModuleInterface } debug; }; - enum OcclusionType { - //! occlusion is not detected - NOT_OCCLUDED, - //! occlusion is not caused by dynamic objects - STATICALLY_OCCLUDED, - //! occlusion is caused by dynamic objects - DYNAMICALLY_OCCLUDED, - //! actual occlusion does not exist, only disapproved by RTC - RTC_OCCLUDED, + //! occlusion is not detected + struct NotOccluded + { + double best_clearance_distance{-1.0}; + }; + //! occlusion is not caused by dynamic objects + struct StaticallyOccluded + { + double best_clearance_distance{0.0}; + }; + //! occlusion is caused by dynamic objects + struct DynamicallyOccluded + { + double best_clearance_distance{0.0}; }; + //! actual occlusion does not exist, only disapproved by RTC + using RTCOccluded = std::monostate; + using OcclusionType = + std::variant; struct DebugData { @@ -294,11 +304,9 @@ class IntersectionModule : public SceneModuleInterface bool getOcclusionSafety() const { return occlusion_safety_; } double getOcclusionDistance() const { return occlusion_stop_distance_; } void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; } - bool isOcclusionFirstStopRequired() { return occlusion_first_stop_required_; } + bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; } private: - rclcpp::Node & node_; - /** *********************************************************** *********************************************************** @@ -396,7 +404,7 @@ class IntersectionModule : public SceneModuleInterface * @defgroup occlusion-variables [var] occlusion detection variables * @{ */ - OcclusionType prev_occlusion_status_; + OcclusionType prev_occlusion_status_{NotOccluded{}}; //! debouncing for the first brief stop at the default stopline StateMachine before_creep_state_machine_; @@ -499,7 +507,7 @@ class IntersectionModule : public SceneModuleInterface */ std::optional getStopLineIndexFromMap( const intersection::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet); + lanelet::ConstLanelet assigned_lanelet) const; /** * @brief generate IntersectionStopLines @@ -510,7 +518,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets @@ -518,7 +526,7 @@ class IntersectionModule : public SceneModuleInterface intersection::IntersectionLanelets generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet); + const lanelet::ConstLanelet assigned_lanelet) const; /** * @brief generate PathLanelets @@ -529,14 +537,15 @@ class IntersectionModule : public SceneModuleInterface const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx); + const std::vector & attention_areas, + const size_t closest_idx) const; /** * @brief generate discretized detection lane linestring. */ std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const; /** @} */ private: @@ -636,7 +645,8 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion(const intersection::InterpolatedPathInfo & interpolated_path_info); + OcclusionType detectOcclusion( + const intersection::InterpolatedPathInfo & interpolated_path_info) const; /** @} */ private: @@ -699,7 +709,7 @@ class IntersectionModule : public SceneModuleInterface */ std::optional isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines); + const intersection::IntersectionStopLines & intersection_stoplines) const; /** * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and @@ -731,7 +741,8 @@ class IntersectionModule : public SceneModuleInterface * @brief return if collision is detected and the collision position */ CollisionStatus detectCollision( - const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line); + const bool is_over_1st_pass_judge_line, + const std::optional is_over_2nd_pass_judge_line) const; std::optional checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, @@ -747,7 +758,7 @@ class IntersectionModule : public SceneModuleInterface TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; /** @} */ mutable DebugData debug_data_; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 4f7e8b45d107f..d7eba93166cfc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -346,7 +346,7 @@ void IntersectionModule::cutPredictPathWithinDuration( std::optional IntersectionModule::isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) + const intersection::IntersectionStopLines & intersection_stoplines) const { // ========================================================================================== // if there are any vehicles on the attention area when ego entered the intersection on green @@ -588,7 +588,8 @@ std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( } IntersectionModule::CollisionStatus IntersectionModule::detectCollision( - const bool is_over_1st_pass_judge_line, const std::optional is_over_2nd_pass_judge_line) + const bool is_over_1st_pass_judge_line, + const std::optional is_over_2nd_pass_judge_line) const { // ========================================================================================== // if collision is detected for multiple objects, we prioritize collision on the first @@ -598,14 +599,18 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( bool collision_at_non_first_lane = false; // ========================================================================================== - // find the objects which is judges as UNSAFE after ego passed pass judge lines. + // find the objects which are judged as UNSAFE after ego passed pass judge lines. // // misjudge_objects are those that were once judged as safe when ego passed the pass judge line // - // too_late_detect objects are those that (1) were not detected when ego passed the pass judge - // line (2) were judged as dangerous at the same time when ego passed the pass judge, which are - // expected to have been detected in the prior iteration because ego could have judged as UNSAFE - // in the prior iteration + // too_late_detect_objects are those that (1) were not detected when ego passed the pass judge + // line (2) were judged as dangerous at the same time when ego passed the pass judge line, which + // means they were expected to have been detected when ego passed the pass judge lines or in the + // prior iteration, because ego could have judged them as UNSAFE if their information was + // available at that time. + // + // that case is both "too late to stop" and "too late to go" for the planner. and basically + // detection side is responsible for this fault // ========================================================================================== std::vector>> misjudge_objects; @@ -617,10 +622,10 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( object_info->predicted_object()); continue; } - if (!object_info->is_unsafe()) { + if (!object_info->unsafe_info()) { continue; } - const auto & unsafe_info = object_info->is_unsafe().value(); + const auto & unsafe_info = object_info->unsafe_info().value(); // ========================================================================================== // if ego is over the pass judge lines, then the visualization as "too_late_objects" or // "misjudge_objects" is more important than that for "unsafe" @@ -986,7 +991,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const { const double intersection_velocity = planner_param_.collision_detection.velocity_profile.default_velocity; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index b7c2d4c12878c..364e585f10757 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -46,9 +46,10 @@ IntersectionModule::getOcclusionStatus( auto occlusion_status = (planner_param_.occlusion.enable && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? detectOcclusion(interpolated_path_info) - : OcclusionType::NOT_OCCLUDED; + : NotOccluded{}; occlusion_stop_state_machine_.setStateWithMarginTime( - occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, + std::holds_alternative(occlusion_status) ? StateMachine::State::GO + : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); // module's detection @@ -56,11 +57,11 @@ IntersectionModule::getOcclusionStatus( const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); // RTC's detection if (ext_occlusion_requested) { - occlusion_status = OcclusionType::RTC_OCCLUDED; + occlusion_status = RTCOccluded{}; } const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); // including approval - if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + if (is_occlusion_state && std::holds_alternative(occlusion_status)) { occlusion_status = prev_occlusion_status_; } else { prev_occlusion_status_ = occlusion_status; @@ -69,7 +70,7 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) + const intersection::InterpolatedPathInfo & interpolated_path_info) const { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); @@ -87,7 +88,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( const auto first_attention_area_idx = util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); if (!first_attention_area_idx) { - return OcclusionType::NOT_OCCLUDED; + return NotOccluded{}; } const auto first_inside_attention_idx_ip_opt = @@ -378,7 +379,7 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( } if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return OcclusionType::NOT_OCCLUDED; + return NotOccluded{min_dist}; } debug_data_.nearest_occlusion_projection = @@ -390,9 +391,9 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object_info->predicted_object()); if (bg::intersects(obj_poly, ego_occlusion_line)) { - return OcclusionType::DYNAMICALLY_OCCLUDED; + return DynamicallyOccluded{min_dist}; } } - return OcclusionType::STATICALLY_OCCLUDED; + return StaticallyOccluded{min_dist}; } } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index c44008db9b08b..8125bc05e43f0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -280,7 +280,7 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL std::optional IntersectionModule::getStopLineIndexFromMap( const intersection::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet) + lanelet::ConstLanelet assigned_lanelet) const { const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); @@ -336,7 +336,7 @@ IntersectionModule::generateIntersectionStopLines( const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; const double stopline_margin = planner_param_.common.default_stopline_margin; @@ -567,7 +567,7 @@ IntersectionModule::generateIntersectionStopLines( intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const lanelet::ConstLanelet assigned_lanelet) + const lanelet::ConstLanelet assigned_lanelet) const { const double detection_area_length = planner_param_.common.attention_area_length; const double occlusion_detection_area_length = @@ -772,7 +772,7 @@ std::optional IntersectionModule::generatePathLanele const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, - const std::vector & attention_areas, const size_t closest_idx) + const std::vector & attention_areas, const size_t closest_idx) const { const double width = planner_data_->vehicle_info_.vehicle_width_m; static constexpr double path_lanelet_interval = 1.5; @@ -840,7 +840,7 @@ std::optional IntersectionModule::generatePathLanele std::vector IntersectionModule::generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const { const double curvature_threshold = planner_param_.occlusion.attention_lane_crop_curvature_threshold; From e62b70948a7a2c810085eb3e63d39cea4426b36b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 5 Mar 2024 15:36:16 +0900 Subject: [PATCH 27/36] fix(intersection): fix possible invalid access (#6542) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection_stuck.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 389c73d651f1e..9a604152f8abb 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -34,6 +34,9 @@ lanelet::LineString3d getLineStringFromArcLength( lanelet::Points3d points; double accumulated_length = 0; size_t start_index = linestring.size(); + if (start_index == 0) { + return lanelet::LineString3d{lanelet::InvalId, points}; + } for (size_t i = 0; i < linestring.size() - 1; i++) { const auto & p1 = linestring[i]; const auto & p2 = linestring[i + 1]; From 22ecd3bacaa1eb28e38861c6aff0e08d8957289e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 17 Apr 2024 15:02:01 +0900 Subject: [PATCH 28/36] feat(intersection): resurrect debug ego-object ttc visualizer (#6829) Signed-off-by: Mamoru Sobue --- .../scripts/ttc.py | 16 +- .../src/scene_intersection.cpp | 13 +- .../src/scene_intersection.hpp | 3 +- .../src/scene_intersection_collision.cpp | 353 ++++-------------- 4 files changed, 107 insertions(+), 278 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py index 6d1593d95f055..1eb6ae1ffafc1 100755 --- a/planning/behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -21,6 +21,7 @@ from threading import Lock import time +from PIL import Image import imageio import matplotlib import matplotlib.pyplot as plt @@ -218,7 +219,19 @@ def plot_world(self): def cleanup(self): if self.args.save: kwargs_write = {"fps": self.args.fps, "quantizer": "nq"} - imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write) + max_size_total = 0 + max_size = None + for image in self.images: + (w, h) = image.size + if w * h > max_size_total: + max_size = image.size + max_size_total = w * h + reshaped = [] + for image in self.images: + reshaped.append(image.resize(max_size)) + + imageio.mimsave("./" + self.args.gif + ".gif", reshaped, **kwargs_write) + print("saved fig") rclpy.shutdown() def on_plot_timer(self): @@ -241,6 +254,7 @@ def on_plot_timer(self): if self.args.save: image = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype="uint8") image = image.reshape(self.fig.canvas.get_width_height()[::-1] + (3,)) + image = Image.fromarray(image.astype(np.uint8)) self.images.append(image) def on_ego_ttc(self, msg): diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index dc9c70a2ebc04..ea51da783ea1e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -222,9 +222,20 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // passed each pass judge line for the first time, save current collision status for late // diagnosis // ========================================================================================== + tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; updateObjectInfoManagerCollision( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, - safely_passed_2nd_judge_line); + safely_passed_2nd_judge_line, &object_ttc_time_array); + { + const auto & debug = planner_param_.debug.ttc; + if ( + std::find(debug.begin(), debug.end(), lane_id_) != debug.end() || + std::find(debug.begin(), debug.end(), -1) != debug.end()) { + ego_ttc_pub_->publish(ego_ttc_time_array); + object_ttc_pub_->publish(object_ttc_time_array); + } + } + for (const auto & object_info : object_info_manager_.attentionObjects()) { if (!object_info->unsafe_info()) { continue; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index e62a58d63f923..0b7dce681f99d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -693,7 +693,8 @@ class IntersectionModule : public SceneModuleInterface void updateObjectInfoManagerCollision( const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, const TrafficPrioritizedLevel & traffic_prioritized_level, - const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time); + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, + tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index d7eba93166cfc..54f94be29d650 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -134,7 +134,8 @@ void IntersectionModule::updateObjectInfoManagerCollision( const intersection::PathLanelets & path_lanelets, const IntersectionModule::TimeDistanceArray & time_distance_array, const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, - const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time) + const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, + tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array) { const auto & intersection_lanelets = intersection_lanelets_.value(); @@ -173,6 +174,25 @@ void IntersectionModule::updateObjectInfoManagerCollision( planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); + constexpr size_t object_debug_size = 57; + { + object_ttc_time_array->stamp = clock_->now(); + object_ttc_time_array->layout.dim.resize(3); + object_ttc_time_array->layout.dim.at(0).label = "objects"; + object_ttc_time_array->layout.dim.at(0).size = + 1; // incremented in the loop, first row is lane_id + object_ttc_time_array->layout.dim.at(1).label = + "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " + "start_time, start_dist, " + "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, " + "last_collision_y, " + "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array->layout.dim.at(1).size = object_debug_size; + for (unsigned i = 0; i < object_debug_size; ++i) { + object_ttc_time_array->data.push_back(lane_id_); + } + } + for (auto & object_info : object_info_manager_.attentionObjects()) { const auto & predicted_object = object_info->predicted_object(); bool safe_under_traffic_control = false; @@ -211,6 +231,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( // ========================================================================================== std::optional unsafe_interval{std::nullopt}; std::optional safe_interval{std::nullopt}; + std::optional> object_debug_info{std::nullopt}; for (const auto & predicted_path_ptr : sorted_predicted_paths) { auto predicted_path = *predicted_path_ptr; @@ -281,10 +302,39 @@ void IntersectionModule::updateObjectInfoManagerCollision( break; } } + auto get_object_info = [&]() { + // debug info + const auto & pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + const auto & shape = predicted_object.shape; + const auto [object_start_itr, object_end_itr] = object_passage_interval.interval_position; + const auto & object_start_pos = object_passage_interval.path.at(object_start_itr).position; + const auto & object_end_pos = object_passage_interval.path.at(object_end_itr).position; + std::vector debug; + debug.reserve(object_debug_size); + + debug.insert( + debug.end(), + {pose.position.x, pose.position.y, tf2::getYaw(pose.orientation), shape.dimensions.x, + shape.dimensions.y, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + 1.0 * static_cast(collision_detected), object_enter_time, object_exit_time, + ego_start_itr->first, ego_start_itr->second, ego_end_itr->first, ego_end_itr->second, + object_start_pos.x, object_start_pos.y, object_end_pos.x, object_end_pos.y}); + for (unsigned i = 0; i < 20; ++i) { + const auto & pos = object_passage_interval.path + .at(std::min(i, object_passage_interval.path.size() - 1)) + .position; + debug.push_back(pos.x); + debug.push_back(pos.y); + } + return debug; + }; + if (collision_detected) { // if judged as UNSAFE, return safe_interval = std::nullopt; unsafe_interval = object_passage_interval; + object_debug_info = get_object_info(); break; } if (!safe_interval) { @@ -293,6 +343,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( // judged UNSAFE during the iteration // ========================================================================================== safe_interval = object_passage_interval; + object_debug_info = get_object_info(); } } object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); @@ -322,6 +373,13 @@ void IntersectionModule::updateObjectInfoManagerCollision( .x // observed_velocity }); } + + // debug + if (object_debug_info) { + const auto & data = object_debug_info.value(); + object_ttc_time_array->layout.dim.at(0).size++; + std::copy(data.begin(), data.end(), std::back_inserter(object_ttc_time_array->data)); + } } } @@ -699,257 +757,6 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } -/* -bool IntersectionModule::checkCollision( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, TargetObjects * target_objects, - const intersection::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stopline_candidate_idx, const double time_delay, - const TrafficPrioritizedLevel & traffic_prioritized_level) -{ - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - - // check collision between target_objects predicted path and ego lane - // cut the predicted path at passing_time - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; - const auto time_distance_array = calcIntersectionPassingTime( - path, closest_idx, last_intersection_stopline_candidate_idx, time_delay, &ego_ttc_time_array); - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - ego_ttc_time_array.stamp = path.header.stamp; - ego_ttc_pub_->publish(ego_ttc_time_array); - } - - const double passing_time = time_distance_array.back().first; - cutPredictPathWithDuration(target_objects, passing_time); - - const auto & concat_lanelets = path_lanelets.all; - const auto closest_arc_coords = getArcCoordinates( - concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const auto & ego_lane = path_lanelets.ego_or_entry2exit; - debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - - // change TTC margin based on ego traffic light color - const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { - if (traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED) { - return std::make_pair( - planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, - planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); - } - if (traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { - return std::make_pair( - planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, - planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); - } - return std::make_pair( - planner_param_.collision_detection.not_prioritized.collision_start_margin_time, - planner_param_.collision_detection.not_prioritized.collision_end_margin_time); - }(); - const auto expectedToStopBeforeStopLine = [&](const TargetObject & target_object) { - if (!target_object.dist_to_stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - if (dist_to_stopline < 0) { - return false; - } - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - return dist_to_stopline > braking_distance; - }; - const auto isTolerableOvershoot = [&](const TargetObject & target_object) { - if ( - !target_object.attention_lanelet || !target_object.dist_to_stopline || - !target_object.stopline) { - return false; - } - const double dist_to_stopline = target_object.dist_to_stopline.value(); - const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; - const double braking_distance = - v * v / - (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light - .object_expected_deceleration)); - if (dist_to_stopline > braking_distance) { - return false; - } - const auto stopline_front = target_object.stopline.value().front(); - const auto stopline_back = target_object.stopline.value().back(); - tier4_autoware_utils::LineString2d object_line; - object_line.emplace_back( - (stopline_front.x() + stopline_back.x()) / 2.0, - (stopline_front.y() + stopline_back.y()) / 2.0); - const auto stopline_mid = object_line.front(); - const auto endpoint = target_object.attention_lanelet.value().centerline().back(); - object_line.emplace_back(endpoint.x(), endpoint.y()); - std::vector intersections; - bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); - if (intersections.empty()) { - return false; - } - const auto collision_point = intersections.front(); - // distance from object expected stop position to collision point - const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; - const double stopline_to_collision = - std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); - const double object2collision = stopline_to_collision - stopline_to_object; - const double margin = - planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; - return (object2collision > margin) || (object2collision < 0); - }; - // check collision between predicted_path and ego_area - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; - object_ttc_time_array.layout.dim.resize(3); - object_ttc_time_array.layout.dim.at(0).label = "objects"; - object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id - object_ttc_time_array.layout.dim.at(1).label = - "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " - "start_time, start_dist, " - "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " - "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; - object_ttc_time_array.layout.dim.at(1).size = 57; - for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { - object_ttc_time_array.data.push_back(lane_id_); - } - bool collision_detected = false; - for (const auto & target_object : target_objects->all_attention_objects) { - const auto & object = target_object.object; - // If the vehicle is expected to stop before their stopline, ignore - const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expected_to_stop_before_stopline) { - debug_data_.amber_ignore_targets.objects.push_back(object); - continue; - } - const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); - if ( - traffic_prioritized_level == TrafficPrioritizedLevel::FULLY_PRIORITIZED && - !expected_to_stop_before_stopline && is_tolerable_overshoot) { - debug_data_.red_overshoot_ignore_targets.objects.push_back(object); - continue; - } - for (const auto & predicted_path : object.kinematics.predicted_paths) { - if ( - predicted_path.confidence < - planner_param_.collision_detection.min_predicted_path_confidence) { - // ignore the predicted path with too low confidence - continue; - } - - // collision point - const auto first_itr = std::adjacent_find( - predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); - }); - if (first_itr == predicted_path.path.cend()) continue; - const auto last_itr = std::adjacent_find( - predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly, &object](const auto & a, const auto & b) { - return bg::intersects(ego_poly, ::createOneStepPolygon(a, b, object.shape)); - }); - if (last_itr == predicted_path.path.crend()) continue; - - // possible collision time interval - 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 - collision_start_margin_time > 0) { - // start of possible ego position in the intersection - start_time_distance_itr = std::lower_bound( - time_distance_array.begin(), time_distance_array.end(), - ref_object_enter_time - collision_start_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (start_time_distance_itr == time_distance_array.end()) { - // ego is already at the exit of intersection when npc is at collision point even if npc - // accelerates so ego's position interval is empty - 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 + collision_end_margin_time, - [](const auto & a, const double b) { return a.first < b; }); - if (end_time_distance_itr == time_distance_array.end()) { - // ego is already passing the intersection, when npc is is at collision point - // so ego's position interval is up to the end of intersection lane - 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 - - planner_data_->vehicle_info_.rear_overhang_m); - const double end_arc_length = std::min( - closest_arc_coords.length + (*end_time_distance_itr).second + - planner_data_->vehicle_info_.max_longitudinal_offset_m, - lanelet::utils::getLaneletLength2d(concat_lanelets)); - - const auto trimmed_ego_polygon = - getPolygonFromArcLength(concat_lanelets, start_arc_length, end_arc_length); - - if (trimmed_ego_polygon.empty()) { - continue; - } - - Polygon2d polygon{}; - for (const auto & p : trimmed_ego_polygon) { - polygon.outer().emplace_back(p.x(), p.y()); - } - bg::correct(polygon); - debug_data_.candidate_collision_ego_lane_polygon = toGeomPoly(polygon); - - for (auto itr = first_itr; itr != last_itr.base(); ++itr) { - const auto footprint_polygon = tier4_autoware_utils::toPolygon2d(*itr, object.shape); - if (bg::intersects(polygon, footprint_polygon)) { - collision_detected = true; - break; - } - } - object_ttc_time_array.layout.dim.at(0).size++; - const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto & shape = object.shape; - object_ttc_time_array.data.insert( - object_ttc_time_array.data.end(), - {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), - shape.dimensions.x, shape.dimensions.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x, - 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, - start_time_distance_itr->first, start_time_distance_itr->second, - end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, - first_itr->position.y, last_itr->position.x, last_itr->position.y}); - for (unsigned i = 0; i < 20; i++) { - const auto & pos = - predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; - object_ttc_time_array.data.push_back(pos.x); - object_ttc_time_array.data.push_back(pos.y); - } - if (collision_detected) { - debug_data_.conflicting_targets.objects.push_back(object); - break; - } - } - } - - if ( - std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != - planner_param_.debug.ttc.end()) { - object_ttc_time_array.stamp = path.header.stamp; - object_ttc_pub_->publish(object_ttc_time_array); - } - - return collision_detected; -} -*/ - std::optional IntersectionModule::checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, @@ -991,7 +798,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const + tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { const double intersection_velocity = planner_param_.collision_detection.velocity_profile.default_velocity; @@ -1038,12 +845,12 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin second_attention_stopline_idx ? second_attention_stopline_idx.value() : std::max(occlusion_stopline_idx, first_attention_stopline_idx); - int assigned_lane_found = false; + bool assigned_lane_found = false; // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; std::optional upstream_stopline{std::nullopt}; - for (size_t i = 0; i < path.points.size() - 1; ++i) { + for (size_t i = 0; i + 1 < path.points.size(); ++i) { auto reference_point = path.points.at(i); // assume backward velocity is current ego velocity if (i < closest_idx) { @@ -1070,12 +877,6 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin return {{0.0, 0.0}}; // has already passed the intersection. } - std::vector> original_path_xy; - for (size_t i = 0; i < reference_path.points.size(); ++i) { - const auto & p = reference_path.points.at(i).point.pose.position; - original_path_xy.emplace_back(p.x, p.y); - } - // apply smoother to reference velocity PathWithLaneId smoothed_reference_path = reference_path; if (!smoothPath(reference_path, smoothed_reference_path, planner_data_)) { @@ -1096,7 +897,8 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { if (upstream_stopline) { - const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; + const auto upstream_stopline_point = + reference_path.points.at(upstream_stopline.value()).point.pose; return motion_utils::findFirstNearestIndexWithSoftConstraints( smoothed_reference_path.points, upstream_stopline_point, planner_data_->ego_nearest_dist_threshold, planner_data_->ego_nearest_yaw_threshold); @@ -1105,7 +907,7 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin } }(); - for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { + for (size_t i = smoothed_path_closest_idx; i + 1 < smoothed_reference_path.points.size(); ++i) { const auto & p1 = smoothed_reference_path.points.at(i); const auto & p2 = smoothed_reference_path.points.at(i + 1); @@ -1129,29 +931,30 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin time_distance_array.emplace_back(passing_time, dist_sum); } - debug_ttc_array->layout.dim.resize(3); - debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; - debug_ttc_array->layout.dim.at(0).size = 5; - debug_ttc_array->layout.dim.at(1).label = "values"; - debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); - debug_ttc_array->data.reserve( - time_distance_array.size() * debug_ttc_array->layout.dim.at(0).size); + ego_ttc_array->stamp = clock_->now(); + ego_ttc_array->layout.dim.resize(3); + ego_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + constexpr size_t ego_debug_size = 5; + ego_ttc_array->layout.dim.at(0).size = ego_debug_size; + ego_ttc_array->layout.dim.at(1).label = "values"; + ego_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + ego_ttc_array->data.reserve(time_distance_array.size() * ego_debug_size); for (unsigned i = 0; i < time_distance_array.size(); ++i) { - debug_ttc_array->data.push_back(lane_id_); + ego_ttc_array->data.push_back(lane_id_); } for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(t); + ego_ttc_array->data.push_back(t); } for (const auto & [t, d] : time_distance_array) { - debug_ttc_array->data.push_back(d); + ego_ttc_array->data.push_back(d); } for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.x); + ego_ttc_array->data.push_back(p.x); } for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size(); ++i) { const auto & p = smoothed_reference_path.points.at(i).point.pose.position; - debug_ttc_array->data.push_back(p.y); + ego_ttc_array->data.push_back(p.y); } return time_distance_array; } From 87337f48b343c49ae3c02b23c8ba4dfafdda43b8 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 23 Oct 2024 20:18:08 +0900 Subject: [PATCH 29/36] minimize diff Signed-off-by: Mamoru Sobue From 1bcf343c3fe137e6c61c17d96763661644a1d9cb Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 22 Oct 2024 12:47:30 +0900 Subject: [PATCH 30/36] PR9119 Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index ea51da783ea1e..b4d75d6c7734b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1293,6 +1293,10 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat std::get(prev_decision_result_); return !state.collision_detected; } + if (std::holds_alternative(prev_decision_result_)) { + const auto & prev_decision = std::get(prev_decision_result_); + return !prev_decision.collision_detected; + } return false; }(); From 3bfca0246ca25b8d15a3dc2810649ef435c69faf Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Thu, 31 Oct 2024 16:20:56 +0900 Subject: [PATCH 31/36] feat(behavior_velocity_traffic_light_module): change threshold velocity of pass judge for preventing sudden stop (#1616) Signed-off-by: tomoya.kimura --- planning/behavior_velocity_traffic_light_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index adc1aacf94f48..f9051259c180b 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -353,7 +353,7 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const 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 slow_velocity = planner_data_->current_velocity->twist.linear.x < 1.0; const bool stoppable = distance_stoppable || slow_velocity; const bool reachable = signed_arc_length < reachable_distance; From c87784d22fd35641e1ac55a3f9116a5420799c58 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Thu, 31 Oct 2024 20:42:34 +0900 Subject: [PATCH 32/36] feat(obstacle_cruise_planner)!: relax unknown stop (#1612) * chore: fix dependency of mult object tracker (#1567) fix dependency of mult object tracker * add relax logic Signed-off-by: Yuki Takagi * define params Signed-off-by: Yuki Takagi * add universe params Signed-off-by: Yuki Takagi * delete null line Signed-off-by: Yuki Takagi * fix spell Signed-off-by: Yuki Takagi * fix Signed-off-by: Yuki Takagi * change universe params Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi Co-authored-by: Shohei Sakai --- .../config/obstacle_cruise_planner.param.yaml | 2 ++ .../include/obstacle_cruise_planner/node.hpp | 2 ++ .../obstacle_cruise_planner/planner_interface.hpp | 7 ++++++- planning/obstacle_cruise_planner/src/node.cpp | 12 ++++++++++-- .../src/planner_interface.cpp | 13 +++++++++++++ 5 files changed, 33 insertions(+), 3 deletions(-) diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index fd97d829b8188..b14593ae1819c 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -222,3 +222,5 @@ sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. + limit_margin_for_unknown: 6.0 + preferred_acc_for_unknown: -6.0 diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 909681255d4e9..a879e90b6be87 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -110,6 +110,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double additional_safe_distance_margin_on_curve_; double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; + double limit_margin_for_unknown_; + double preferred_acc_for_unknown_; std::vector stop_obstacle_types_; std::vector inside_cruise_obstacle_types_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 7a060657e16af..d79780a6ee66b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -65,7 +65,8 @@ class PlannerInterface const bool enable_debug_info, const bool enable_calculation_time_info, const double min_behavior_stop_margin, const double enable_approaching_on_curve, const double additional_safe_distance_margin_on_curve, - const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) + const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop, + const double limit_margin_for_unknown, const double preferred_acc_for_unknown) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; @@ -74,6 +75,8 @@ class PlannerInterface additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve; suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop; + limit_margin_for_unknown_ = limit_margin_for_unknown; + preferred_acc_for_unknown_ = preferred_acc_for_unknown; } std::vector generateStopTrajectory( @@ -123,6 +126,8 @@ class PlannerInterface double additional_safe_distance_margin_on_curve_; double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; + double limit_margin_for_unknown_; + double preferred_acc_for_unknown_; // stop watch tier4_autoware_utils::StopWatch< diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 5d924452e8172..c1f47bdb7b656 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -445,10 +445,13 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & declare_parameter("common.stop_on_curve.min_safe_distance_margin"); suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); + limit_margin_for_unknown_ = declare_parameter("stop.limit_margin_for_unknown"); + preferred_acc_for_unknown_ = declare_parameter("stop.preferred_acc_for_unknown"); planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, - min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_, limit_margin_for_unknown_, + preferred_acc_for_unknown_); } { // stop/cruise/slow down obstacle type @@ -496,11 +499,16 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( tier4_autoware_utils::updateParam( parameters, "common.stop_on_curve.min_safe_distance_margin", min_safe_distance_margin_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "stop.limit_margin_for_unknown", limit_margin_for_unknown_); + tier4_autoware_utils::updateParam( + parameters, "stop.preferred_acc_for_unknown", preferred_acc_for_unknown_); planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, - min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_, limit_margin_for_unknown_, + preferred_acc_for_unknown_); tier4_autoware_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index c7ac3c3c63a40..1d1a39d0af282 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -300,6 +300,19 @@ std::vector PlannerInterface::generateStopTrajectory( // calc stop point against the obstacle double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin); + if (stop_obstacle.classification.label == ObjectClassification::UNKNOWN) { + const double pref_acc_stop_dist = + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, preferred_acc_for_unknown_); + const double limit_margin_stop_dist = + std::max(0.0, dist_to_collide_on_ref_traj - limit_margin_for_unknown_); + if (pref_acc_stop_dist > candidate_zero_vel_dist) { + candidate_zero_vel_dist = std::min(pref_acc_stop_dist, limit_margin_stop_dist); + } + } + if (suppress_sudden_obstacle_stop_) { const auto acceptable_stop_acc = [&]() -> std::optional { if (stop_param_.getParamType(stop_obstacle.classification) == "default") { From 9e8f2adba240082a19c42839d8053adefb1dde04 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 5 Nov 2024 20:03:47 +0900 Subject: [PATCH 33/36] fix(crosswalk): don't use vehicle stop checker to remove unnecessary callback (#9234) Signed-off-by: satoshi-ota --- .../src/scene_crosswalk.cpp | 5 +---- .../src/scene_crosswalk.hpp | 2 -- 2 files changed, 1 insertion(+), 6 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 169d42d7955e5..b1a6b45d83cf0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -197,8 +197,6 @@ CrosswalkModule::CrosswalkModule( collision_info_pub_ = node.create_publisher("~/debug/collision_info", 1); - - vehicle_stop_checker_ = std::make_unique(&node); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -1221,8 +1219,7 @@ void CrosswalkModule::planStop( bool CrosswalkModule::checkRestartSuppression( const PathWithLaneId & ego_path, const std::optional & stop_factor) const { - const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped(); - if (!is_vehicle_stopped) { + if (!planner_data_->isVehicleStopped()) { return false; } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 77b422f1bd5da..f21e6883c4a3e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -436,8 +436,6 @@ class CrosswalkModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; - std::unique_ptr vehicle_stop_checker_{nullptr}; - // Stop watch StopWatch stop_watch_; From 3cacd75e3314c12edc0ca8dbe998b1556289c476 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 1 Nov 2024 18:52:48 +0900 Subject: [PATCH 34/36] fix(bvp): remove expired module safely (#9212) * fix(bvp): remove expired module safely Signed-off-by: satoshi-ota * fix: remove module id set Signed-off-by: satoshi-ota * fix: use itr to erase expired module Signed-off-by: satoshi-ota * fix: remove unused function Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../src/manager.cpp | 20 +- .../src/manager.cpp | 600 ++++++++++++++++++ .../scene_module_interface.hpp | 2 - .../src/scene_module_interface.cpp | 41 +- 4 files changed, 625 insertions(+), 38 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 79d31352ab911..fe35e37920bc5 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -303,20 +303,20 @@ void IntersectionModuleManager::deleteExpiredModules( { 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)) { + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { // default - removeRTCStatus(getUUID(scene_module->getModuleId())); - removeUUID(scene_module->getModuleId()); + removeRTCStatus(getUUID((*itr)->getModuleId())); + removeUUID((*itr)->getModuleId()); // occlusion - const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto intersection_module = std::dynamic_pointer_cast(*itr); const auto occlusion_uuid = intersection_module->getOcclusionUUID(); occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); - unregisterModule(scene_module); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp new file mode 100644 index 0000000000000..dcb0ebb5ba688 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -0,0 +1,600 @@ +// 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 "manager.hpp" + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::universe_utils::getOrDeclareParameter; + +IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterfaceWithRTC( + node, getModuleName(), + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection")), + occlusion_rtc_interface_( + &node, "intersection_occlusion", + getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc.intersection_to_occlusion")) +{ + const std::string ns(IntersectionModuleManager::getModuleName()); + auto & ip = intersection_param_; + + // common + { + ip.common.attention_area_length = + getOrDeclareParameter(node, ns + ".common.attention_area_length"); + ip.common.attention_area_margin = + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); + ip.common.attention_area_angle_threshold = + getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); + ip.common.use_intersection_area = + getOrDeclareParameter(node, ns + ".common.use_intersection_area"); + ip.common.default_stopline_margin = + getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); + ip.common.stopline_overshoot_margin = + getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); + ip.common.path_interpolation_ds = + getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); + ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + ip.common.delay_response_time = + getOrDeclareParameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); + } + + // stuck + { + // target_type + { + ip.stuck_vehicle.target_type.car = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.car"); + ip.stuck_vehicle.target_type.bus = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bus"); + ip.stuck_vehicle.target_type.truck = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.truck"); + ip.stuck_vehicle.target_type.trailer = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.trailer"); + ip.stuck_vehicle.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.motorcycle"); + ip.stuck_vehicle.target_type.bicycle = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.bicycle"); + ip.stuck_vehicle.target_type.unknown = + getOrDeclareParameter(node, ns + ".stuck_vehicle.target_type.unknown"); + } + + // turn_direction + { + ip.stuck_vehicle.turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + ip.stuck_vehicle.turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + ip.stuck_vehicle.turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); + } + + ip.stuck_vehicle.use_stuck_stopline = + getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); + ip.stuck_vehicle.stuck_vehicle_detect_dist = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); + ip.stuck_vehicle.stuck_vehicle_velocity_threshold = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); + ip.stuck_vehicle.disable_against_private_lane = + getOrDeclareParameter(node, ns + ".stuck_vehicle.disable_against_private_lane"); + } + + // yield_stuck + { + // target_type + { + ip.yield_stuck.target_type.car = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.car"); + ip.yield_stuck.target_type.bus = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bus"); + ip.yield_stuck.target_type.truck = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.truck"); + ip.yield_stuck.target_type.trailer = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.trailer"); + ip.yield_stuck.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.motorcycle"); + ip.yield_stuck.target_type.bicycle = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.bicycle"); + ip.yield_stuck.target_type.unknown = + getOrDeclareParameter(node, ns + ".yield_stuck.target_type.unknown"); + } + + // turn_direction + { + ip.yield_stuck.turn_direction.left = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); + ip.yield_stuck.turn_direction.right = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.right"); + ip.yield_stuck.turn_direction.straight = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.straight"); + } + + ip.yield_stuck.distance_threshold = + getOrDeclareParameter(node, ns + ".yield_stuck.distance_threshold"); + } + + // collision_detection + { + ip.collision_detection.consider_wrong_direction_vehicle = getOrDeclareParameter( + node, ns + ".collision_detection.consider_wrong_direction_vehicle"); + ip.collision_detection.collision_detection_hold_time = getOrDeclareParameter( + node, ns + ".collision_detection.collision_detection_hold_time"); + ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter( + node, ns + ".collision_detection.min_predicted_path_confidence"); + + // target_type + { + ip.collision_detection.target_type.car = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.car"); + ip.collision_detection.target_type.bus = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bus"); + ip.collision_detection.target_type.truck = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.truck"); + ip.collision_detection.target_type.trailer = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.trailer"); + ip.collision_detection.target_type.motorcycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.motorcycle"); + ip.collision_detection.target_type.bicycle = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.bicycle"); + ip.collision_detection.target_type.unknown = + getOrDeclareParameter(node, ns + ".collision_detection.target_type.unknown"); + } + + // velocity_profile + { + ip.collision_detection.velocity_profile.use_upstream = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.use_upstream"); + ip.collision_detection.velocity_profile.minimum_upstream_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_upstream_velocity"); + ip.collision_detection.velocity_profile.default_velocity = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.default_velocity"); + ip.collision_detection.velocity_profile.minimum_default_velocity = + getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_default_velocity"); + } + + // fully_prioritized + { + ip.collision_detection.fully_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); + ip.collision_detection.fully_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); + } + + // partially_prioritized + { + ip.collision_detection.partially_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); + ip.collision_detection.partially_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); + } + + // not_prioritized + { + ip.collision_detection.not_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); + ip.collision_detection.not_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); + } + + // yield_on_green_traffic_light + { + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = + getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); + } + + // ignore_on_amber_traffic_light, ignore_on_red_traffic_light + { + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.car"); + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike = + getOrDeclareParameter( + node, + ns + + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration.bike"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); + } + + ip.collision_detection.avoid_collision_by_acceleration.object_time_margin_to_collision_point = + getOrDeclareParameter( + node, ns + + ".collision_detection.avoid_collision_by_acceleration.object_time_margin_to_" + "collision_point"); + } + + // occlusion + { + ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + ip.occlusion.occlusion_attention_area_length = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); + ip.occlusion.free_space_max = + getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + ip.occlusion.denoise_kernel = + getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); + ip.occlusion.attention_lane_crop_curvature_threshold = getOrDeclareParameter( + node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = getOrDeclareParameter( + node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + + // creep_during_peeking + { + ip.occlusion.creep_during_peeking.enable = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.enable"); + ip.occlusion.creep_during_peeking.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); + } + + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); + ip.occlusion.occlusion_required_clearance_distance = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_required_clearance_distance"); + ip.occlusion.possible_object_bbox = + getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); + ip.occlusion.ignore_parked_vehicle_speed_threshold = + getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.occlusion_detection_hold_time = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_detection_hold_time"); + ip.occlusion.temporal_stop_time_before_peeking = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); + ip.occlusion.temporal_stop_before_attention_area = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.creep_velocity_without_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.creep_velocity_without_traffic_light"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + } + + ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); + + decision_state_pub_ = + node.create_publisher("~/debug/intersection/decision_state", 1); + tl_observation_pub_ = node.create_publisher( + "~/debug/intersection_traffic_signal", 1); +} + +void IntersectionModuleManager::launchNewModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + // run occlusion detection only in the first intersection + 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; + + // 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; + } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { + continue; + } + + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + bool has_traffic_light = false; + if (const auto tl_reg_elems = ll.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; + } + const auto new_module = std::make_shared( + module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_); + generateUUID(module_id); + /* set RTC status as non_occluded status initially */ + const UUID uuid = getUUID(new_module->getModuleId()); + const auto occlusion_uuid = new_module->getOcclusionUUID(); + rtc_interface_.updateCooperateStatus( + uuid, true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + std::numeric_limits::lowest(), clock_->now()); + registerModule(std::move(new_module)); + } +} + +std::function &)> +IntersectionModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = intersection_module->getAssociativeIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (associative_ids.find(lane.id()) != associative_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = intersection_module->getAssociativeIds(); + if (associative_ids.find(lane.id()) != associative_ids.end()) { + return true; + } + } + return false; +} + +void IntersectionModuleManager::sendRTC(const Time & stamp) +{ + double min_distance = std::numeric_limits::infinity(); + std::optional nearest_tl_observation{std::nullopt}; + std_msgs::msg::String decision_type; + + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const UUID uuid = getUUID(scene_module->getModuleId()); + const bool safety = + scene_module->isSafe() && (!intersection_module->isOcclusionFirstStopRequired()); + updateRTCStatus(uuid, safety, State::RUNNING, scene_module->getDistance(), stamp); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + const auto occlusion_distance = intersection_module->getOcclusionDistance(); + const auto occlusion_safety = intersection_module->getOcclusionSafety(); + occlusion_rtc_interface_.updateCooperateStatus( + occlusion_uuid, occlusion_safety, State::RUNNING, occlusion_distance, occlusion_distance, + stamp); + + // ========================================================================================== + // module debug data + // ========================================================================================== + const auto internal_debug_data = intersection_module->getInternalDebugData(); + if (internal_debug_data.distance < min_distance) { + min_distance = internal_debug_data.distance; + nearest_tl_observation = internal_debug_data.tl_observation; + } + decision_type.data += (internal_debug_data.decision_type + "\n"); + } + rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() + occlusion_rtc_interface_.publishCooperateStatus(stamp); + + // ========================================================================================== + // publish module debug data + // ========================================================================================== + decision_state_pub_->publish(decision_type); + if (nearest_tl_observation) { + tl_observation_pub_->publish(nearest_tl_observation.value().signal); + } +} + +void IntersectionModuleManager::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const auto intersection_module = std::dynamic_pointer_cast(scene_module); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + scene_module->setActivation(rtc_interface_.isActivated(getUUID(scene_module->getModuleId()))); + intersection_module->setOcclusionActivation( + occlusion_rtc_interface_.isActivated(occlusion_uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(getUUID(scene_module->getModuleId()))); + } +} + +void IntersectionModuleManager::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + // default + removeRTCStatus(getUUID((*itr)->getModuleId())); + removeUUID((*itr)->getModuleId()); + // occlusion + const auto intersection_module = std::dynamic_pointer_cast(*itr); + const auto occlusion_uuid = intersection_module->getOcclusionUUID(); + occlusion_rtc_interface_.removeCooperateStatus(occlusion_uuid); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } +} + +MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + const std::string ns(MergeFromPrivateModuleManager::getModuleName()); + auto & mp = merge_from_private_area_param_; + mp.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); + mp.attention_area_length = + node.get_parameter("intersection.common.attention_area_length").as_double(); + mp.stopline_margin = getOrDeclareParameter(node, ns + ".stopline_margin"); + mp.path_interpolation_ds = + node.get_parameter("intersection.common.path_interpolation_ds").as_double(); + mp.stop_distance_threshold = getOrDeclareParameter(node, ns + ".stop_distance_threshold"); +} + +void MergeFromPrivateModuleManager::launchNewModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); + + const auto lanelets = + planning_utils::getLaneletsOnPath(path, lanelet_map, planner_data_->current_odometry->pose); + 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? + // In case the goal is in private road, check if this lanelet is conflicting with urban lanelet + const std::string lane_location = ll.attributeOr("location", "else"); + if (lane_location != "private") { + continue; + } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll)) { + continue; + } + + if (i + 1 < lanelets.size()) { + const auto next_lane = lanelets.at(i + 1); + const std::string next_lane_location = next_lane.attributeOr("location", "else"); + if (next_lane_location != "private") { + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } else { + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll); + for (auto && conflicting_lanelet : conflicting_lanelets) { + const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); + if (conflicting_attr == "urban") { + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, associative_ids, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } + } + } +} + +std::function &)> +MergeFromPrivateModuleManager::getModuleExpiredFunction( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto lane_set = planning_utils::getLaneletsOnPath( + path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + return [this, lane_set](const std::shared_ptr & scene_module) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = merge_from_private_module->getAssociativeIds(); + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; + } + + if (associative_ids.find(lane.id()) != associative_ids.end() /* contains */) { + return false; + } + } + return true; + }; +} + +bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane) const +{ + for (const auto & scene_module : scene_modules_) { + const auto merge_from_private_module = + std::dynamic_pointer_cast(scene_module); + const auto & associative_ids = merge_from_private_module->getAssociativeIds(); + if (associative_ids.find(lane.id()) != associative_ids.end()) { + return true; + } + } + return false; +} + +} // namespace autoware::behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::IntersectionModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) +PLUGINLIB_EXPORT_CLASS( + autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index fcde16d8a871c..13a3e2dc7e1d9 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -185,8 +185,6 @@ class SceneModuleManagerInterface void registerModule(const std::shared_ptr & scene_module); - void unregisterModule(const std::shared_ptr & scene_module); - size_t findEgoSegmentIndex( const std::vector & points) const; diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 362493005eb17..a284500b1a591 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -160,13 +160,12 @@ void SceneModuleManagerInterface::deleteExpiredModules( { 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); + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + itr = scene_modules_.erase(itr); + } else { + itr++; } } } @@ -180,16 +179,6 @@ void SceneModuleManagerInterface::registerModule( scene_modules_.insert(scene_module); } -void SceneModuleManagerInterface::unregisterModule( - const std::shared_ptr & scene_module) -{ - RCLCPP_DEBUG( - logger_, "unregister task: module = %s, id = %lu", getModuleName(), - scene_module->getModuleId()); - registered_module_id_set_.erase(scene_module->getModuleId()); - scene_modules_.erase(scene_module); -} - SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc) : SceneModuleManagerInterface(node, module_name), @@ -265,15 +254,15 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( { 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)) { - removeRTCStatus(getUUID(scene_module->getModuleId())); - removeUUID(scene_module->getModuleId()); - unregisterModule(scene_module); + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + removeRTCStatus(getUUID((*itr)->getModuleId())); + removeUUID((*itr)->getModuleId()); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; } } } From 98f9724d4fec3ea3b1d834bca45569253f8dfc3d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 7 Nov 2024 19:39:23 +0900 Subject: [PATCH 35/36] fix(lane_change): enable cancel when ego in turn direction lane (RT0-33893) (#1594) * yield at intersection by making the check conservative Signed-off-by: Zulfaqar Azmi * make code of similar to main branch Signed-off-by: Zulfaqar Azmi * fix build error Signed-off-by: Zulfaqar Azmi * fix logic Signed-off-by: Zulfaqar Azmi * update README Signed-off-by: Zulfaqar Azmi * change variable name Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../README.md | 1 + .../config/lane_change.param.yaml | 2 + .../scene.hpp | 3 + .../utils/data_structs.hpp | 1 + .../utils/path.hpp | 5 ++ .../utils/utils.hpp | 10 +++ .../src/interface.cpp | 1 + .../src/manager.cpp | 2 + .../src/scene.cpp | 72 +++++++++++++++++++ .../src/utils/utils.cpp | 67 +++++++++++++++++ 10 files changed, 164 insertions(+) diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index dc077ab02cae6..29b7f39490699 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -341,6 +341,7 @@ The following parameters are configurable in `lane_change.param.yaml`. | `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | +| `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | | `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | | `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | | `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml index 1ab33514c5f24..c9dc2922b2078 100644 --- a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml @@ -6,6 +6,8 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] + backward_length_from_intersection: 5.0 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 04f21f5249240..3ae70cbdae91d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -186,6 +186,9 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } + void update_dist_from_intersection(); + + std::vector path_after_intersection_; double stop_time_{0.0}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index 5039dea7c0db6..2903a0afbc978 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -93,6 +93,7 @@ struct LaneChangeParameters // lane change parameters double backward_length_buffer_for_end_of_lane; double backward_length_buffer_for_blocking_object; + double backward_length_from_intersection{5.0}; double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.0}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 9e42b49635b82..913077eac0413 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -21,6 +21,7 @@ #include +#include #include namespace behavior_path_planner @@ -42,11 +43,15 @@ struct LaneChangeStatus LaneChangePath lane_change_path{}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; + lanelet::ConstLanelet ego_lane{}; std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; bool is_valid_path{true}; + bool is_ego_in_turn_direction_lane{false}; + bool is_ego_in_intersection{false}; double start_distance{0.0}; + double dist_from_prev_intersection{std::numeric_limits::max()}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 48b940093c2cd..f8ca377ccac65 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -43,6 +43,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; @@ -338,5 +339,14 @@ bool has_blocking_target_object( const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects, const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose, const PathWithLaneId & path); + +bool has_passed_intersection_turn_direction( + const LaneChangeParameters & lc_param, const LaneChangeStatus & status); + +std::vector get_line_string_paths(const ExtendedPredictedObject & object); + +bool has_overtaking_turn_lane_object( + const LaneChangeStatus & status, const LaneChangeParameters & lc_param, + const ExtendedPredictedObjects & trailing_objects); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 61699a8b35bdc..c734e845d850c 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -120,6 +120,7 @@ ModuleStatus LaneChangeInterface::updateState() module_type_->evaluateApprovedPathWithUnsafeHysteresis(module_type_->isApprovedPathSafe()); setObjectDebugVisualization(); + if (is_safe) { log_warn_throttled("Lane change path is safe."); module_type_->toNormalState(); diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index a854a4984d261..3cba445642de9 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -169,6 +169,8 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); p.backward_length_buffer_for_blocking_object = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + p.backward_length_from_intersection = + getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); p.lane_changing_lateral_jerk = getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); p.lane_change_prepare_duration = diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 284ea9409e924..69c4a939767bb 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -54,6 +54,27 @@ NormalLaneChange::NormalLaneChange( void NormalLaneChange::updateLaneChangeStatus() { updateStopTime(); + status_.current_lanes = getCurrentLanes(); + if (status_.current_lanes.empty()) { + return; + } + + lanelet::ConstLanelet current_lane; + if (!getRouteHandler()->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_DEBUG(logger_, "ego's current lane not in route"); + return; + } + status_.ego_lane = current_lane; + + const auto ego_footprint = + utils::lane_change::getEgoCurrentFootprint(getEgoPose(), getCommonParam().vehicle_info); + status_.is_ego_in_turn_direction_lane = + utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); + status_.is_ego_in_intersection = + utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint); + + update_dist_from_intersection(); + const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -1372,6 +1393,15 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); + if (status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane) { + return false; + } + if (utils::lane_change::has_overtaking_turn_lane_object( + status_, *lane_change_parameters_, target_objects.target_lane)) { + RCLCPP_DEBUG(logger_, "has overtaking object while ego leaves intersection"); + return false; + } + std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( @@ -1415,6 +1445,12 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto target_objects = getTargetObjects(current_lanes, target_lanes); debug_filtered_objects_ = target_objects; + const auto has_overtaking_object = utils::lane_change::has_overtaking_turn_lane_object( + status_, *lane_change_parameters_, target_objects.target_lane); + + if (has_overtaking_object) { + return {false, true}; + } CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); const auto safety_status = isLaneChangePathSafe( @@ -1890,4 +1926,40 @@ bool NormalLaneChange::check_prepare_phase() const return check_in_intersection || check_in_turns || check_in_general_lanes; } +void NormalLaneChange::update_dist_from_intersection() +{ + if ( + status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane && + path_after_intersection_.empty()) { + const auto target_neighbor = + utils::lane_change::getTargetNeighborLanes(*getRouteHandler(), status_.current_lanes, type_); + auto path_after_intersection = getRouteHandler()->getCenterLinePath( + target_neighbor, 0.0, std::numeric_limits::max()); + path_after_intersection_ = std::move(path_after_intersection.points); + status_.dist_from_prev_intersection = 0.0; + return; + } + + if ( + status_.is_ego_in_intersection || status_.is_ego_in_turn_direction_lane || + path_after_intersection_.empty()) { + return; + } + + const auto & path_points = path_after_intersection_; + const auto & front_point = path_points.front().point.pose.position; + const auto & ego_position = getEgoPosition(); + status_.dist_from_prev_intersection = calcSignedArcLength(path_points, front_point, ego_position); + + if ( + status_.dist_from_prev_intersection >= 0.0 && + status_.dist_from_prev_intersection <= + lane_change_parameters_->backward_length_from_intersection) { + return; + } + + path_after_intersection_.clear(); + status_.dist_from_prev_intersection = std::numeric_limits::max(); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 0d5b20db3c0c8..56cb451ce8f14 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1305,4 +1305,71 @@ bool has_blocking_target_object( return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; }); } + +bool has_passed_intersection_turn_direction( + const LaneChangeParameters & lc_param, const LaneChangeStatus & status) +{ + if (status.is_ego_in_intersection && status.is_ego_in_turn_direction_lane) { + return false; + } + + return status.dist_from_prev_intersection > lc_param.backward_length_from_intersection; +} + +std::vector get_line_string_paths(const ExtendedPredictedObject & object) +{ + const auto transform = [](const auto & predicted_path) -> LineString2d { + LineString2d line_string; + const auto & path = predicted_path.path; + line_string.reserve(path.size()); + for (const auto & path_point : path) { + const auto point = tier4_autoware_utils::fromMsg(path_point.pose.position).to_2d(); + line_string.push_back(point); + } + + return line_string; + }; + + const auto paths = object.predicted_paths; + std::vector line_strings; + std::transform(paths.begin(), paths.end(), std::back_inserter(line_strings), transform); + + return line_strings; +} + +bool has_overtaking_turn_lane_object( + const LaneChangeStatus & status, const LaneChangeParameters & lc_param, + const ExtendedPredictedObjects & trailing_objects) +{ + // Note: This situation is only applicable if the ego is in a turn lane. + if (has_passed_intersection_turn_direction(lc_param, status)) { + return false; + } + + const auto & target_lanes = status.target_lanes; + const auto & ego_current_lane = status.ego_lane; + + const auto target_lane_poly = + lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon(); + // to compensate for perception issue, or if object is from behind ego, and tries to overtake, + // but stop all of sudden + const auto is_overlap_with_target = [&](const LineString2d & path) { + return !boost::geometry::disjoint(path, target_lane_poly); + }; + + return std::any_of( + trailing_objects.begin(), trailing_objects.end(), [&](const ExtendedPredictedObject & object) { + lanelet::ConstLanelet obj_lane; + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + if (!boost::geometry::disjoint( + obj_poly, utils::toPolygon2d( + lanelet::utils::to2D(ego_current_lane.polygon2d().basicPolygon())))) { + return true; + } + + const auto paths = get_line_string_paths(object); + return std::any_of(paths.begin(), paths.end(), is_overlap_with_target); + }); +} } // namespace behavior_path_planner::utils::lane_change From 0f32e2e4fb980554c1b73e5dfd8f972865608ef0 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 7 Nov 2024 19:40:14 +0900 Subject: [PATCH 36/36] fix(lane_change): delay lane change cancel (#8048) (for RT0-33952) (#1626) fix(lane_change): delay lane change cancel (#8048) RT1-6955: delay lane change cancel Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 20 ++++++++++++++++++- .../src/utils/utils.cpp | 10 +++++----- 2 files changed, 24 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 69c4a939767bb..373568549b98d 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1405,7 +1405,7 @@ bool NormalLaneChange::getLaneChangePaths( std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && utils::lane_change::passParkedObject( + !is_stuck && !utils::lane_change::passParkedObject( route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route, *lane_change_parameters_, object_debug_)) { debug_print( @@ -1453,6 +1453,24 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const } CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); + + const auto & route_handler = *getRouteHandler(); + const auto & lc_param = *lane_change_parameters_; + + const auto min_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const auto is_goal_in_route = route_handler.isInGoalRouteSection(current_lanes.back()); + + const auto has_passed_parked_objects = utils::lane_change::passParkedObject( + route_handler, path, target_objects.target_lane, min_lc_length, is_goal_in_route, lc_param, + debug_data); + + if (!has_passed_parked_objects) { + RCLCPP_DEBUG(logger_, "Lane change has been delayed."); + return {false, false}; + } + const auto safety_status = isLaneChangePathSafe( path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 56cb451ce8f14..39557200eac2e 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -991,14 +991,14 @@ bool passParkedObject( route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); if (objects.empty() || path.points.empty() || current_lane_path.points.empty()) { - return false; + return true; } const auto leading_obj_idx = getLeadingStaticObjectIdx( route_handler, lane_change_path, objects, object_check_min_road_shoulder_width, object_shiftable_ratio_threshold); if (!leading_obj_idx) { - return false; + return true; } const auto & leading_obj = objects.at(*leading_obj_idx); @@ -1006,7 +1006,7 @@ bool passParkedObject( const auto leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { - return false; + return true; } const auto & current_path_end = current_lane_path.points.back().point.pose.position; @@ -1028,10 +1028,10 @@ bool passParkedObject( if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { debug.second.unsafe_reason = "delay lane change"; utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); - return true; + return false; } - return false; + return true; } std::optional getLeadingStaticObjectIdx(