diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index 6433f1b9ae2f9..99f7dd4333bae 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -28,15 +28,18 @@ using geometry_msgs::msg::Pose; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index 331b3dd5275fd..3f92f41b8738d 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -38,6 +38,7 @@ struct VirtualWall std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; + bool is_driving_forward{true}; }; typedef std::vector VirtualWalls; @@ -54,7 +55,7 @@ class VirtualWallMarkerCreator using create_wall_function = std::function; + const std::string & ns_prefix, const bool is_driving_forward)>; VirtualWalls virtual_walls; std::unordered_map marker_count_per_namespace; diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index 25d8616949b02..7998dbeabe7f9 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -86,10 +86,11 @@ namespace motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); @@ -97,10 +98,11 @@ visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); @@ -108,10 +110,11 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index 63c456ab24fb0..87736f8a2982b 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -64,7 +64,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( } auto markers = create_fn( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns); + virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = marker_count_per_namespace[marker.ns].current++; marker_array.markers.push_back(marker); diff --git a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp index 4446c87427e88..31892853a855f 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/transform.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/transform.hpp @@ -15,15 +15,22 @@ #ifndef OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ #define OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ +#include + #include +#include +#include #include #include #include #ifdef ROS_DISTRO_GALACTIC +#include #include #else +#include + #include #endif @@ -45,6 +52,23 @@ namespace detail return boost::none; } } + +[[maybe_unused]] inline boost::optional getTransformMatrix( + const std::string & in_target_frame, const std_msgs::msg::Header & in_cloud_header, + const tf2_ros::Buffer & tf_buffer) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + in_target_frame, in_cloud_header.frame_id, in_cloud_header.stamp, + rclcpp::Duration::from_seconds(1.0)); + Eigen::Matrix4f mat = tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + return mat; + } catch (tf2::TransformException & e) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("detail::getTransformMatrix"), e.what()); + return boost::none; + } +} } // namespace detail namespace object_recognition_utils @@ -79,6 +103,46 @@ bool transformObjects( } return true; } +template +bool transformObjectsWithFeature( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + const auto tf_matrix = detail::getTransformMatrix(target_frame_id, input_msg.header, tf_buffer); + if (!tf_matrix) { + RCLCPP_WARN( + rclcpp::get_logger("object_recognition_utils:"), "failed to get transformed matrix"); + return false; + } + for (auto & feature_object : output_msg.feature_objects) { + // transform object + tf2::fromMsg( + feature_object.object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, feature_object.object.kinematics.pose_with_covariance.pose); + + // transform cluster + sensor_msgs::msg::PointCloud2 transformed_cluster; + pcl_ros::transformPointCloud(*tf_matrix, feature_object.feature.cluster, transformed_cluster); + transformed_cluster.header.frame_id = target_frame_id; + feature_object.feature.cluster = transformed_cluster; + } + output_msg.header.frame_id = target_frame_id; + return true; + } + return true; +} } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__TRANSFORM_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 95925e846a55c..2f2472515ebad 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -17,7 +17,13 @@ geometry_msgs interpolation libboost-dev + pcl_conversions + pcl_ros rclcpp + sensor_msgs + std_msgs + tf2 + tf2_eigen tier4_autoware_utils ament_cmake_ros diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index ff448e694eb15..058a5d5deb5d6 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -366,7 +366,10 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->clearContents(); num_rtc_status_ptr_->setText( QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); - if (msg->statuses.empty()) return; + if (msg->statuses.empty()) { + rtc_table_->update(); + return; + } // this is to stable rtc display not to occupy too much size_t min_display_size{5}; size_t max_display_size{10}; @@ -374,8 +377,17 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->setRowCount( std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); int cnt = 0; - for (auto status : msg->statuses) { - if (static_cast(cnt) >= max_display_size) return; + + auto sorted_statuses = msg->statuses; + std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { + return !status.auto_mode && !uint2bool(status.command_status.type); + }); + + for (auto status : sorted_statuses) { + if (static_cast(cnt) >= max_display_size) { + rtc_table_->update(); + return; + } // uuid { std::stringstream uuid; diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index f8434bbdb081f..fe15a5dd8593b 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -121,14 +121,24 @@ bool AutonomousMode::isModeChangeCompleted() const auto closest_point = trajectory_.points.at(*closest_idx); // check for lateral deviation - const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const auto dist_deviation = + motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + if (std::isnan(dist_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (dist_deviation > stable_check_param_.dist_threshold) { RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation); return unstable(); } // check for yaw deviation - const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const auto yaw_deviation = + motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + if (std::isnan(yaw_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (yaw_deviation > stable_check_param_.yaw_threshold) { RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); return unstable(); diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 12961c1470eb9..cf18f6c691878 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -1,8 +1,24 @@ + + + + - + + + + + + + + + + + + + @@ -28,16 +44,6 @@ - - - - - - - - - - - + - + + @@ -68,13 +75,54 @@ - - + + - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -89,6 +137,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 3a24ec2fdf500..2bd007b658978 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -1,70 +1,66 @@ - + + + + - - - + + + + + + - - + + + - + + + + + + + + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - + @@ -80,10 +76,321 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ca5d1d0f7e8bb..ab55047132482 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -5,7 +5,6 @@ - @@ -65,6 +64,8 @@ + + @@ -94,6 +95,8 @@ + + @@ -107,6 +110,7 @@ + @@ -119,6 +123,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 6d67df5fa7a1f..ebe96200b6883 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -4,37 +4,33 @@ - - - + - + + - - - + - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index a47e6a86f1c7b..4756fc9338e46 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -3,7 +3,6 @@ - @@ -19,6 +18,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index a5d8d2113d2db..93d395ca3e466 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -35,7 +35,6 @@ def __init__(self, context): ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] @@ -46,47 +45,40 @@ def __init__(self, context): ] self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] + self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): - if self.use_down_sample_filter: - return self.create_down_sample_pipeline() + if self.use_pointcloud_map == "true": + return self.create_compare_map_pipeline() else: - return self.create_normal_pipeline() + return self.create_no_compare_map_pipeline() - def create_normal_pipeline(self): + def create_no_compare_map_pipeline(self): components = [] components.append( ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), - ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, - "input_frame": "map", + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, } ], extra_arguments=[ - {"use_intra_process_comms": False}, + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], - ) + ), ) return components - def create_down_sample_pipeline(self): + def create_compare_map_pipeline(self): components = [] down_sample_topic = ( "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" @@ -94,7 +86,7 @@ def create_down_sample_pipeline(self): components.append( ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 204fb561b10ea..acc14b1dcbe24 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,10 +1,8 @@ - - @@ -58,8 +56,9 @@ - + + - - - + + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index c1723c1fa07e8..336588891d9b2 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator detected_object_feature_remover diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt new file mode 100644 index 0000000000000..49506f4b439fb --- /dev/null +++ b/perception/cluster_merger/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.8) +project(cluster_merger) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(cluster_merger_node_component SHARED + src/cluster_merger/node.cpp +) + +rclcpp_components_register_node(cluster_merger_node_component + PLUGIN "cluster_merger::ClusterMergerNode" + EXECUTABLE cluster_merger_node) + + +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/perception/cluster_merger/README.md b/perception/cluster_merger/README.md new file mode 100644 index 0000000000000..b46f7401b70ec --- /dev/null +++ b/perception/cluster_merger/README.md @@ -0,0 +1,72 @@ +# cluster merger + +## Purpose + +cluster_merger is a package for merging pointcloud clusters as detected objects with feature type. + +## Inner-working / Algorithms + +The clusters of merged topics are simply concatenated from clusters of input topics. + +## Input / Output + +### Input + +| Name | Type | Description | +| ---------------- | -------------------------------------------------------- | ------------------- | +| `input/cluster0` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | +| `input/cluster1` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | pointcloud clusters | + +### Output + +| Name | Type | Description | +| ----------------- | ----------------------------------------------------- | --------------- | +| `output/clusters` | `autoware_auto_perception_msgs::msg::DetectedObjects` | merged clusters | + +## Parameters + +| Name | Type | Description | Default value | +| :---------------- | :----- | :----------------------------------- | :------------ | +| `output_frame_id` | string | The header frame_id of output topic. | base_link | + +## Assumptions / Known limits + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp new file mode 100644 index 0000000000000..8da5999f00384 --- /dev/null +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -0,0 +1,73 @@ +// 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. + +#ifndef CLUSTER_MERGER__NODE_HPP_ +#define CLUSTER_MERGER__NODE_HPP_ + +#include "message_filters/subscriber.h" +#include "message_filters/sync_policies/approximate_time.h" +#include "message_filters/synchronizer.h" +#include "rclcpp/rclcpp.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" + +#include +#include + +#include +#include +#include +#include + +namespace cluster_merger +{ +using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using tier4_perception_msgs::msg::DetectedObjectWithFeature; + +class ClusterMergerNode : public rclcpp::Node +{ +public: + explicit ClusterMergerNode(const rclcpp::NodeOptions & node_options); + +private: + // Subscriber + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + rclcpp::Subscription::SharedPtr sub_objects_{}; + message_filters::Subscriber objects0_sub_; + message_filters::Subscriber objects1_sub_; + typedef message_filters::sync_policies::ApproximateTime< + DetectedObjectsWithFeature, DetectedObjectsWithFeature> + SyncPolicy; + typedef message_filters::Synchronizer Sync; + Sync sync_; + + std::string output_frame_id_; + + std::vector::SharedPtr> sub_objects_array{}; + std::shared_ptr transform_listener_; + + void objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg); + // Publisher + rclcpp::Publisher::SharedPtr pub_objects_; +}; + +} // namespace cluster_merger + +#endif // CLUSTER_MERGER__NODE_HPP_ diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml new file mode 100644 index 0000000000000..1bbd0ebd91e12 --- /dev/null +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/perception/cluster_merger/package.xml b/perception/cluster_merger/package.xml new file mode 100644 index 0000000000000..14826ad07e098 --- /dev/null +++ b/perception/cluster_merger/package.xml @@ -0,0 +1,28 @@ + + + + cluster_merger + 0.1.0 + The ROS 2 cluster merger package + Yukihiro Saito + Shunsuke Miura + autoware + Apache License 2.0 + + ament_cmake_auto + + geometry_msgs + message_filters + object_recognition_utils + rclcpp + rclcpp_components + tier4_autoware_utils + tier4_perception_msgs + autoware_cmake + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/cluster_merger/src/cluster_merger/node.cpp b/perception/cluster_merger/src/cluster_merger/node.cpp new file mode 100644 index 0000000000000..48bf953027510 --- /dev/null +++ b/perception/cluster_merger/src/cluster_merger/node.cpp @@ -0,0 +1,73 @@ +// 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. + +#include "cluster_merger/node.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" + +#include +#include +#include +namespace cluster_merger +{ + +ClusterMergerNode::ClusterMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("cluster_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_), + objects0_sub_(this, "input/cluster0", rclcpp::QoS{1}.get_rmw_qos_profile()), + objects1_sub_(this, "input/cluster1", rclcpp::QoS{1}.get_rmw_qos_profile()), + sync_(SyncPolicy(10), objects0_sub_, objects1_sub_) +{ + output_frame_id_ = declare_parameter("output_frame_id"); + using std::placeholders::_1; + using std::placeholders::_2; + sync_.registerCallback(std::bind(&ClusterMergerNode::objectsCallback, this, _1, _2)); + + // Publisher + pub_objects_ = create_publisher("~/output/clusters", rclcpp::QoS{1}); +} + +void ClusterMergerNode::objectsCallback( + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects0_msg, + const DetectedObjectsWithFeature::ConstSharedPtr & input_objects1_msg) +{ + if (pub_objects_->get_subscription_count() < 1) { + return; + } + // TODO(badai-nguyen): transform input topics to desired frame before concatenating + /* transform to the same with cluster0 frame id*/ + DetectedObjectsWithFeature transformed_objects0; + DetectedObjectsWithFeature transformed_objects1; + if ( + !object_recognition_utils::transformObjectsWithFeature( + *input_objects0_msg, output_frame_id_, tf_buffer_, transformed_objects0) || + !object_recognition_utils::transformObjectsWithFeature( + *input_objects1_msg, output_frame_id_, tf_buffer_, transformed_objects1)) { + return; + } + + DetectedObjectsWithFeature output_objects; + output_objects.header = input_objects0_msg->header; + // add check frame id and transform if they are different + output_objects.feature_objects = transformed_objects0.feature_objects; + output_objects.feature_objects.insert( + output_objects.feature_objects.end(), transformed_objects1.feature_objects.begin(), + transformed_objects1.feature_objects.end()); + pub_objects_->publish(output_objects); +} +} // namespace cluster_merger + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(cluster_merger::ClusterMergerNode) diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml deleted file mode 100644 index 3dd303464a2c1..0000000000000 --- a/perception/euclidean_cluster/config/compare_map.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - - # distance threshold for compare compare - distance_threshold: 0.5 - - # publish voxelized map pointcloud for debug - publish_debug_pcd: False - - # use dynamic map loading - use_dynamic_map_loading: True - - # time interval to check dynamic map loading - timer_interval_ms: 100 - - # distance threshold for dynamic map update - map_update_distance_threshold: 10.0 - - # radius map for dynamic map loading - map_loader_radius: 150.0 diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml deleted file mode 100644 index 1962fba1f332a..0000000000000 --- a/perception/euclidean_cluster/config/outlier.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.3 - voxel_size_y: 0.3 - voxel_size_z: 100.0 - voxel_points_threshold: 3 diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml deleted file mode 100644 index 3ff32bfbb7146..0000000000000 --- a/perception/euclidean_cluster/config/voxel_grid.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.15 - voxel_size_y: 0.15 - voxel_size_z: 0.15 diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 2f3de2b789eb3..26b027f0077aa 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -7,9 +7,11 @@ max_cluster_size: 3000 use_height: false input_frame: "base_link" - max_x: 70.0 - min_x: -70.0 - max_y: 70.0 - min_y: -70.0 - max_z: 4.5 - min_z: -4.5 + + # low height crop box filter param + max_x: 200.0 + min_x: -200.0 + max_y: 200.0 + min_y: -200.0 + max_z: 2.0 + min_z: -10.0 diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index db86bbf80d250..66c25396a656e 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -35,61 +35,35 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "voxel_grid_filtered/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), - ("map", LaunchConfiguration("input_map")), - ("output", "compare_map_filtered/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[ - { - "distance_threshold": 0.5, - "timer_interval_ms": 100, - "use_dynamic_map_loading": True, - "downsize_ratio_z_axis": 0.5, - "map_update_distance_threshold": 10.0, - "map_loader_radius": 150.0, - "publish_debug_pcd": True, - "input_frame": "map", - } + ("output", "low_height/pointcloud"), ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_euclidean_cluster_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "compare_map_filtered/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], ) - disuse_map_euclidean_cluster_component = ComposableNode( + disuse_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), + ("input", LaunchConfiguration("input_pointcloud")), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], @@ -100,26 +74,26 @@ def load_composable_node_param(param_path): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[voxel_grid_filter_component], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_euclidean_cluster_component], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) - return [container, use_map_loader, disuse_map_loader] + return [container, use_low_height_pointcloud_loader, disuse_low_height_pointcloud_loader] def generate_launch_description(): @@ -131,14 +105,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index 7159fb4c42793..fd1ea82befae0 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -3,16 +3,14 @@ - - + - - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 82ce5605b67cd..00bcd4422bd3c 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,10 +16,8 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution -from launch.substitutions import AnonName +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -36,159 +34,31 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - namespace=ns, - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("map", LaunchConfiguration("input_map")), - ("output", "map_filter/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[load_composable_node_param("compare_map_param_path")], - ) - - # separate range of pointcloud when map_filter used - use_map_short_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", - remappings=[ - ("input", "map_filter/pointcloud"), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - use_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # disuse_map_voxel_grid_filter - disuse_map_short_range_crop_box_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - disuse_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[ - ("input", "short_range/pointcloud"), - ("output", "downsampled/short_range/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set outlier filter as a component - outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", - name="outlier_filter", - remappings=[ - ("input", "downsampled/short_range/pointcloud"), - ("output", "outlier_filter/pointcloud"), - ], - parameters=[load_composable_node_param("outlier_param_path")], - ) - - # concat with-outlier pointcloud and without-outlier pcl - downsample_concat_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concat_downsampled_pcl", - remappings=[("output", "downsampled/concatenated/pointcloud")], - parameters=[ - { - "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], - "output_frame": "base_link", - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", "downsampled/concatenated/pointcloud"), - ("output", LaunchConfiguration("output_clusters")), + ("output", "low_height/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_disuse_downsample_euclidean_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "map_filter/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - disuse_map_disuse_downsample_euclidean_component = ComposableNode( + + disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -209,75 +79,24 @@ def load_composable_node_param(param_path): output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_short_range_crop_box_filter_component, - use_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_short_range_crop_box_filter_component, - disuse_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, - ], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), - ) - - use_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - compare_map_filter_component, - use_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - (LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - - disuse_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ container, - use_map_use_downsample_loader, - disuse_map_use_downsample_loader, - use_map_disuse_downsample_loader, - disuse_map_disuse_downsample_loader, + use_low_height_pointcloud_loader, + disuse_low_height_pointcloud_loader, ] @@ -290,29 +109,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg("use_downsample_pointcloud", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), - add_launch_arg( - "outlier_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/outlier.param.yaml", - ], - ), - add_launch_arg( - "compare_map_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/compare_map.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 2833fed81c28e..b6a426dabfd12 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -3,22 +3,14 @@ - - - - - + - - - - - + diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index 906ad71d21732..ce7c3b5ea12a9 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -22,6 +22,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/utils.cpp src/roi_cluster_fusion/node.cpp src/roi_detected_object_fusion/node.cpp + src/roi_pointcloud_fusion/node.cpp ) target_link_libraries(${PROJECT_NAME} @@ -39,6 +40,11 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE roi_cluster_fusion_node ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "image_projection_based_fusion::RoiPointCloudFusionNode" + EXECUTABLE roi_pointcloud_fusion_node +) + set(CUDA_VERBOSE OFF) # set flags for CUDA availability diff --git a/perception/image_projection_based_fusion/README.md b/perception/image_projection_based_fusion/README.md index 1bbee35ab44f8..207989d8a6f25 100644 --- a/perception/image_projection_based_fusion/README.md +++ b/perception/image_projection_based_fusion/README.md @@ -58,3 +58,4 @@ The rclcpp::TimerBase timer could not break a for loop, therefore even if time i | roi_cluster_fusion | Overwrite a classification label of clusters by that of ROIs from a 2D object detector. | [link](./docs/roi-cluster-fusion.md) | | roi_detected_object_fusion | Overwrite a classification label of detected objects by that of ROIs from a 2D object detector. | [link](./docs/roi-detected-object-fusion.md) | | pointpainting_fusion | Paint the point cloud with the ROIs from a 2D object detector and feed to a 3D object detector. | [link](./docs/pointpainting-fusion.md) | +| roi_pointcloud_fusion | Matching pointcloud with ROIs from a 2D object detector to detect unknown-labeled objects | [link](./docs/roi-pointcloud-fusion.md) | diff --git a/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png new file mode 100644 index 0000000000000..c529de7c728fb Binary files /dev/null and b/perception/image_projection_based_fusion/docs/images/roi_pointcloud_fusion.png differ diff --git a/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md new file mode 100644 index 0000000000000..5d0b241a578d6 --- /dev/null +++ b/perception/image_projection_based_fusion/docs/roi-pointcloud-fusion.md @@ -0,0 +1,93 @@ +# roi_pointcloud_fusion + +## Purpose + +The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of Interests (ROIs) detected by a 2D object detector, specific for unknown labeled ROI. + +## Inner-workings / Algorithms + +- The pointclouds are projected onto image planes and extracted as cluster if they are inside the unknown labeled ROIs. +- Since the cluster might contain unrelated points from background, then a refinement step is added to filter the background pointcloud by distance to camera. + +![roi_pointcloud_fusion_image](./images/roi_pointcloud_fusion.png) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | +| `input` | `sensor_msgs::msg::PointCloud2` | input pointcloud | +| `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | +| `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | +| `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | + +### Output + +| Name | Type | Description | +| ----------------- | -------------------------------------------------------- | -------------------------------------------- | +| `output` | `sensor_msgs::msg::PointCloud2` | output pointcloud as default of interface | +| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | +| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | + +## Parameters + +### Core Parameters + +| Name | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------------------------------- | +| `min_cluster_size` | int | the minimum number of points that a cluster needs to contain in order to be considered valid | +| `cluster_2d_tolerance` | double | cluster tolerance measured in radial direction | +| `rois_number` | int | the number of input rois | + +## Assumptions / Known limits + +- Currently, the refinement is only based on distance to camera, the roi based clustering is supposed to work well with small object ROIs. Others criteria for refinement might needed in the future. + + + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## (Optional) References/External links + + + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 016633fa4ef92..5174aebe069a8 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -29,6 +29,9 @@ #include #include #include +#include +#include +#include #include #include @@ -49,6 +52,8 @@ using autoware_auto_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::PointCloud2; using tier4_perception_msgs::msg::DetectedObjectsWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; +using PointCloud = pcl::PointCloud; +using autoware_auto_perception_msgs::msg::ObjectClassification; template class FusionNode : public rclcpp::Node diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 38f9e71eea02d..70a7866e79b87 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -43,13 +43,13 @@ class RoiClusterFusionNode bool use_iou_{false}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; - float roi_scale_factor_{1.1f}; - float iou_threshold_{0.0f}; - float unknown_iou_threshold_{0.0f}; + double roi_scale_factor_{1.1}; + double iou_threshold_{0.0}; + double unknown_iou_threshold_{0.0}; const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - float trust_distance_; + double trust_distance_; bool filter_by_distance(const DetectedObjectWithFeature & obj); bool out_of_scope(const DetectedObjectWithFeature & obj); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp new file mode 100644 index 0000000000000..2b4eb822a9edb --- /dev/null +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -0,0 +1,53 @@ +// 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. + +#ifndef IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ +#define IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ + +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +namespace image_projection_based_fusion +{ +class RoiPointCloudFusionNode +: public FusionNode +{ +private: + int min_cluster_size_{1}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; + + rclcpp::Publisher::SharedPtr pub_objects_ptr_; + std::vector output_fused_objects_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; + + /* data */ +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + +protected: + void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; + bool out_of_scope(const DetectedObjectWithFeature & obj); +}; + +} // namespace image_projection_based_fusion +#endif // IMAGE_PROJECTION_BASED_FUSION__ROI_POINTCLOUD_FUSION__NODE_HPP_ diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp index 541cf997412c2..d7fd3c3882046 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/utils.hpp @@ -32,18 +32,54 @@ #include #endif +#include "image_projection_based_fusion/fusion_node.hpp" + +#include +#include +#include +#include + +#include "autoware_auto_perception_msgs/msg/shape.hpp" +#include "tier4_perception_msgs/msg/detected_object_with_feature.hpp" + +#include +#include +#include + #include #include +#include namespace image_projection_based_fusion { +using PointCloud = pcl::PointCloud; std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj); +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center); + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects); + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster); +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj); + } // namespace image_projection_based_fusion #endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__UTILS_HPP_ diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 302b567942e84..b2b9bae80f071 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -49,9 +49,10 @@ + - + diff --git a/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml new file mode 100644 index 0000000000000..181f4ccb88320 --- /dev/null +++ b/perception/image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/image_projection_based_fusion/package.xml b/perception/image_projection_based_fusion/package.xml index 32d7a5633b811..5ff99af2ebb21 100644 --- a/perception/image_projection_based_fusion/package.xml +++ b/perception/image_projection_based_fusion/package.xml @@ -14,6 +14,7 @@ autoware_auto_perception_msgs autoware_point_types cv_bridge + euclidean_cluster image_transport lidar_centerpoint message_filters diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index 9ce34d8482fbd..447e66f076c0b 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -420,4 +420,5 @@ void FusionNode::publish(const Msg & output_msg) template class FusionNode; template class FusionNode; template class FusionNode; +template class FusionNode; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9180b18adeed8..e9cb76b050427 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x", true); - use_iou_y_ = declare_parameter("use_iou_y", false); - use_iou_ = declare_parameter("use_iou", false); - use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); - only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster_", true); - roi_scale_factor_ = declare_parameter("roi_scale_factor", 1.1); - iou_threshold_ = declare_parameter("iou_threshold", 0.1); - unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold", 0.1); - remove_unknown_ = declare_parameter("remove_unknown", false); - trust_distance_ = declare_parameter("trust_distance", 100.0); + use_iou_x_ = declare_parameter("use_iou_x"); + use_iou_y_ = declare_parameter("use_iou_y"); + use_iou_ = declare_parameter("use_iou"); + use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); + only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); + roi_scale_factor_ = declare_parameter("roi_scale_factor"); + iou_threshold_ = declare_parameter("iou_threshold"); + unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); + remove_unknown_ = declare_parameter("remove_unknown"); + trust_distance_ = declare_parameter("trust_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -171,12 +171,16 @@ void RoiClusterFusionNode::fuseOnSingleImage( for (const auto & feature_obj : input_roi_msg.feature_objects) { int index = 0; double max_iou = 0.0; + bool is_roi_label_known = + feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { double iou(0.0), iou_x(0.0), iou_y(0.0); if (use_iou_) { iou = calcIoU(cluster_map.second, feature_obj.feature.roi); } - if (use_iou_x_) { + // use for unknown roi to improve small objects like traffic cone detect + // TODO(badai-nguyen): add option to disable roi_cluster mode + if (use_iou_x_ || !is_roi_label_known) { iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); } if (use_iou_y_) { @@ -192,8 +196,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } if (!output_cluster_msg.feature_objects.empty()) { - bool is_roi_label_known = feature_obj.object.classification.front().label != - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; bool is_roi_existence_prob_higher = output_cluster_msg.feature_objects.at(index).object.existence_probability <= feature_obj.object.existence_probability; diff --git a/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp new file mode 100644 index 0000000000000..233e568ebee0b --- /dev/null +++ b/perception/image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -0,0 +1,165 @@ +// 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. + +#include "image_projection_based_fusion/roi_pointcloud_fusion/node.hpp" + +#include "image_projection_based_fusion/utils/geometry.hpp" +#include "image_projection_based_fusion/utils/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif + +#include "euclidean_cluster/utils.hpp" + +namespace image_projection_based_fusion +{ +RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) +: FusionNode("roi_pointcloud_fusion", options) +{ + fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); + min_cluster_size_ = declare_parameter("min_cluster_size"); + cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); + pub_objects_ptr_ = + this->create_publisher("output_clusters", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); +} + +void RoiPointCloudFusionNode::preprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + return; +} + +void RoiPointCloudFusionNode::postprocess(__attribute__((unused)) + sensor_msgs::msg::PointCloud2 & pointcloud_msg) +{ + const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + + pub_objects_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + DetectedObjectsWithFeature output_msg; + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + if (objects_sub_count > 0) { + pub_objects_ptr_->publish(output_msg); + } + output_fused_objects_.clear(); + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + sensor_msgs::msg::PointCloud2 debug_cluster_msg; + euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } +} +void RoiPointCloudFusionNode::fuseOnSingleImage( + const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, + __attribute__((unused)) const std::size_t image_id, + const DetectedObjectsWithFeature & input_roi_msg, + const sensor_msgs::msg::CameraInfo & camera_info, + __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) +{ + if (input_pointcloud_msg.data.empty()) { + return; + } + std::vector output_objs; + // select ROIs for fusion + for (const auto & feature_obj : input_roi_msg.feature_objects) { + if (fuse_unknown_only_) { + bool is_roi_label_unknown = feature_obj.object.classification.front().label == + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (is_roi_label_unknown) { + output_objs.push_back(feature_obj); + } + } else { + // TODO(badai-nguyen): selected class from a list + output_objs.push_back(feature_obj); + } + } + if (output_objs.empty()) { + return; + } + + // transform pointcloud to camera optical frame id + Eigen::Matrix4d projection; + projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3), + camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7), + camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11); + geometry_msgs::msg::TransformStamped transform_stamped; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer_, input_roi_msg.header.frame_id, input_pointcloud_msg.header.frame_id, + input_roi_msg.header.stamp); + if (!transform_stamped_optional) { + return; + } + transform_stamped = transform_stamped_optional.value(); + } + + sensor_msgs::msg::PointCloud2 transformed_cloud; + tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); + + std::vector clusters; + clusters.resize(output_objs.size()); + + for (sensor_msgs::PointCloud2ConstIterator iter_x(transformed_cloud, "x"), + iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), + iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), + iter_orig_z(input_pointcloud_msg, "z"); + iter_x != iter_x.end(); + ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + if (*iter_z <= 0.0) { + continue; + } + Eigen::Vector4d projected_point = projection * Eigen::Vector4d(*iter_x, *iter_y, *iter_z, 1.0); + Eigen::Vector2d normalized_projected_point = Eigen::Vector2d( + projected_point.x() / projected_point.z(), projected_point.y() / projected_point.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + if ( + check_roi.x_offset <= normalized_projected_point.x() && + check_roi.y_offset <= normalized_projected_point.y() && + check_roi.x_offset + check_roi.width >= normalized_projected_point.x() && + check_roi.y_offset + check_roi.height >= normalized_projected_point.y()) { + cluster.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + } + } + } + + // refine and update output_fused_objects_ + updateOutputFusedObjects( + output_objs, clusters, input_pointcloud_msg.header, input_roi_msg.header, tf_buffer_, + min_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); +} + +bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) + const DetectedObjectWithFeature & obj) +{ + return false; +} +} // namespace image_projection_based_fusion + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(image_projection_based_fusion::RoiPointCloudFusionNode) diff --git a/perception/image_projection_based_fusion/src/utils/utils.cpp b/perception/image_projection_based_fusion/src/utils/utils.cpp index 670c5596001fb..76cd1e3c61e82 100644 --- a/perception/image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/image_projection_based_fusion/src/utils/utils.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "image_projection_based_fusion/utils/utils.hpp" - namespace image_projection_based_fusion { @@ -39,4 +38,197 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) return a; } +void convertCluster2FeatureObject( + const std_msgs::msg::Header & header, const PointCloud & cluster, + DetectedObjectWithFeature & feature_obj) +{ + PointCloud2 ros_cluster; + pcl::toROSMsg(cluster, ros_cluster); + ros_cluster.header = header; + feature_obj.feature.cluster = ros_cluster; + feature_obj.object.kinematics.pose_with_covariance.pose.position = getCentroid(ros_cluster); + feature_obj.object.existence_probability = 1.0f; +} + +PointCloud closest_cluster( + PointCloud & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center) +{ + // sort point by distance to camera origin + + auto func = [center](const pcl::PointXYZ & p1, const pcl::PointXYZ & p2) { + return tier4_autoware_utils::calcDistance2d(center, p1) < + tier4_autoware_utils::calcDistance2d(center, p2); + }; + std::sort(cluster.begin(), cluster.end(), func); + PointCloud out_cluster; + for (auto & point : cluster) { + if (out_cluster.empty()) { + out_cluster.push_back(point); + continue; + } + if (tier4_autoware_utils::calcDistance2d(out_cluster.back(), point) < cluster_2d_tolerance) { + out_cluster.push_back(point); + continue; + } + if (out_cluster.size() >= static_cast(min_cluster_size)) { + return out_cluster; + } + out_cluster.clear(); + out_cluster.push_back(point); + } + return out_cluster; +} + +void updateOutputFusedObjects( + std::vector & output_objs, const std::vector & clusters, + const std_msgs::msg::Header & in_cloud_header, const std_msgs::msg::Header & in_roi_header, + const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const float cluster_2d_tolerance, + std::vector & output_fused_objects) +{ + if (output_objs.size() != clusters.size()) { + return; + } + Eigen::Vector3d orig_camera_frame, orig_point_frame; + Eigen::Affine3d camera2lidar_affine; + orig_camera_frame << 0.0, 0.0, 0.0; + { + const auto transform_stamped_optional = getTransformStamped( + tf_buffer, in_cloud_header.frame_id, in_roi_header.frame_id, in_roi_header.stamp); + if (!transform_stamped_optional) { + return; + } + camera2lidar_affine = transformToEigen(transform_stamped_optional.value().transform); + } + orig_point_frame = camera2lidar_affine * orig_camera_frame; + pcl::PointXYZ camera_orig_point_frame = + pcl::PointXYZ(orig_point_frame.x(), orig_point_frame.y(), orig_point_frame.z()); + + for (std::size_t i = 0; i < output_objs.size(); ++i) { + PointCloud cluster = clusters.at(i); + auto & feature_obj = output_objs.at(i); + if (cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest + // to output refine cluster and centroid + auto refine_cluster = + closest_cluster(cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame); + if (refine_cluster.points.size() < std::size_t(min_cluster_size)) { + continue; + } + + refine_cluster.width = refine_cluster.points.size(); + refine_cluster.height = 1; + refine_cluster.is_dense = false; + // convert cluster to object + convertCluster2FeatureObject(in_cloud_header, refine_cluster, feature_obj); + output_fused_objects.push_back(feature_obj); + } +} + +void addShapeAndKinematic( + const pcl::PointCloud & cluster, + tier4_perception_msgs::msg::DetectedObjectWithFeature & feature_obj) +{ + if (cluster.empty()) { + return; + } + pcl::PointXYZ centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + float max_z = -1e6; + float min_z = 1e6; + for (const auto & point : cluster) { + centroid.x += point.x; + centroid.y += point.y; + centroid.z += point.z; + max_z = max_z < point.z ? point.z : max_z; + min_z = min_z > point.z ? point.z : min_z; + } + centroid.x = centroid.x / static_cast(cluster.size()); + centroid.y = centroid.y / static_cast(cluster.size()); + centroid.z = centroid.z / static_cast(cluster.size()); + + std::vector cluster2d; + std::vector cluster2d_convex; + + for (size_t i = 0; i < cluster.size(); ++i) { + cluster2d.push_back( + cv::Point((cluster.at(i).x - centroid.x) * 1000.0, (cluster.at(i).y - centroid.y) * 1000.)); + } + cv::convexHull(cluster2d, cluster2d_convex); + if (cluster2d_convex.empty()) { + return; + } + pcl::PointXYZ polygon_centroid = pcl::PointXYZ(0.0, 0.0, 0.0); + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + polygon_centroid.x += static_cast(cluster2d_convex.at(i).x) / 1000.0; + polygon_centroid.y += static_cast(cluster2d_convex.at(i).y) / 1000.0; + } + polygon_centroid.x = polygon_centroid.x / static_cast(cluster2d_convex.size()); + polygon_centroid.y = polygon_centroid.y / static_cast(cluster2d_convex.size()); + + autoware_auto_perception_msgs::msg::Shape shape; + for (size_t i = 0; i < cluster2d_convex.size(); ++i) { + geometry_msgs::msg::Point32 point; + point.x = cluster2d_convex.at(i).x / 1000.0; + point.y = cluster2d_convex.at(i).y / 1000.0; + point.z = 0.0; + shape.footprint.points.push_back(point); + } + shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + constexpr float eps = 0.01; + shape.dimensions.x = 0; + shape.dimensions.y = 0; + shape.dimensions.z = std::max((max_z - min_z), eps); + feature_obj.object.shape = shape; + feature_obj.object.kinematics.pose_with_covariance.pose.position.x = + centroid.x + polygon_centroid.x; + feature_obj.object.kinematics.pose_with_covariance.pose.position.y = + centroid.y + polygon_centroid.y; + feature_obj.object.kinematics.pose_with_covariance.pose.position.z = + min_z + shape.dimensions.z * 0.5; + feature_obj.object.existence_probability = 1.0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.x = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.y = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.z = 0; + feature_obj.object.kinematics.pose_with_covariance.pose.orientation.w = 1; +} + +geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +{ + geometry_msgs::msg::Point centroid; + centroid.x = 0.0f; + centroid.y = 0.0f; + centroid.z = 0.0f; + for (sensor_msgs::PointCloud2ConstIterator iter_x(pointcloud, "x"), + iter_y(pointcloud, "y"), iter_z(pointcloud, "z"); + iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { + centroid.x += *iter_x; + centroid.y += *iter_y; + centroid.z += *iter_z; + } + const size_t size = pointcloud.width * pointcloud.height; + centroid.x = centroid.x / static_cast(size); + centroid.y = centroid.y / static_cast(size); + centroid.z = centroid.z / static_cast(size); + return centroid; +} + +pcl::PointXYZ getClosestPoint(const pcl::PointCloud & cluster) +{ + pcl::PointXYZ closest_point; + double min_dist = 1e6; + pcl::PointXYZ orig_point = pcl::PointXYZ(0.0, 0.0, 0.0); + for (std::size_t i = 0; i < cluster.points.size(); ++i) { + pcl::PointXYZ point = cluster.points.at(i); + double dist_closest_point = tier4_autoware_utils::calcDistance2d(point, orig_point); + if (min_dist > dist_closest_point) { + min_dist = dist_closest_point; + closest_point = pcl::PointXYZ(point.x, point.y, point.z); + } + } + return closest_point; +} + } // namespace image_projection_based_fusion diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 01ade9bac43d6..80a7f0964b4bc 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -30,7 +30,7 @@ set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") if(NOT EXISTS "${DATA_PATH}") execute_process(COMMAND mkdir -p ${DATA_PATH}) endif() -function(download FILE_NAME FILE_HASH) +function(download SUB_DIR FILE_NAME FILE_HASH) message(STATUS "Checking and downloading ${FILE_NAME}") set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) set(STATUS_CODE 0) @@ -44,14 +44,14 @@ function(download FILE_NAME FILE_HASH) else() message(STATUS "diff ${FILE_NAME}") message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/${SUB_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() else() message(STATUS "not found ${FILE_NAME}") message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) + file(DOWNLOAD https://awf.ml.dev.web.auto/perception/${SUB_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) list(GET DOWNLOAD_STATUS 0 STATUS_CODE) list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) endif() @@ -62,10 +62,13 @@ function(download FILE_NAME FILE_HASH) endif() endfunction() -download(yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) -download(yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) -download(yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) -download(label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) +set(V1_PATH models/object_detection_yolox_s/v1) +download(models yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) +download(models yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) +download(models yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) +download(${V1_PATH} yolox-sPlus-T4-960x960-pseudo-finetune.onnx 37165a7e67bdaf6acff93925c628f2b2) +download(${V1_PATH} yolox-sPlus-T4-960x960-pseudo-finetune.EntropyV2-calibration.table d266ea9846b10e4dab53572152c6fd8f) +download(models label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) ########## # tensorrt_yolox diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 43f59e944f889..cb89f5829c65d 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,8 +1,9 @@ + - + diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 0352096d02b2b..f62e418371401 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -32,6 +32,7 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/path_utils.cpp src/utils/path_safety_checker/safety_check.cpp src/utils/path_safety_checker/objects_filtering.cpp + src/utils/start_goal_planner_common/utils.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 8d413f54817fc..c6072e8a50ce3 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -142,10 +142,20 @@ # collision check parameters check_all_predicted_path: false # [-] time_resolution: 0.5 # [s] - time_horizon: 10.0 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] - safety_check_hysteresis_factor: 2.0 # [-] safety_check_ego_offset: 1.0 # [m] + hysteresis_factor_expand_rate: 2.0 # [-] + hysteresis_factor_safe_count: 10 # [-] + # rss parameters + expected_front_deceleration: -1.0 # [m/ss] + expected_rear_deceleration: -1.0 # [m/ss] + rear_vehicle_reaction_time: 2.0 # [s] + rear_vehicle_safety_time_margin: 1.0 # [s] + lateral_distance_max_threshold: 2.0 # [m] + longitudinal_distance_min_threshold: 3.0 # [m] + longitudinal_velocity_delta_time: 0.8 # [s] # For avoidance maneuver avoidance: @@ -161,6 +171,7 @@ # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] + remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 02dc13ffd84da..95c60612bfdb7 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,19 +25,6 @@ visualize_maximum_drivable_area: true - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 # [s] - - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lane_following: drivable_area_right_bound_offset: 0.0 drivable_area_left_bound_offset: 0.0 diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..779e7af61699f 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,7 +2,6 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -17,25 +16,28 @@ longitudinal_margin: 3.0 max_lateral_offset: 0.5 lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 + ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 # occupancy grid map occupancy_grid: - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false + use_occupancy_grid_for_goal_search: true + use_occupancy_grid_for_goal_longitudinal_margin: false + use_occupancy_grid_for_path_collision_check: false occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 # object recognition object_recognition: - use_object_recognition: true + use_object_recognition: false object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 + th_moving_object_velocity: 1.0 # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 @@ -104,6 +106,68 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon with the value + hysteresis_factor_expand_rate: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 8fabe8daa85b2..544471d9f8a91 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -27,6 +27,18 @@ min_longitudinal_acc: -1.0 max_longitudinal_acc: 1.0 + # safety check + safety_check: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + expected_front_deceleration_for_abort: -1.0 + expected_rear_deceleration_for_abort: -2.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index e89c5539a0965..9895d9b5e473f 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -8,6 +8,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -16,6 +17,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 6 max_module_size: 1 @@ -24,6 +26,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 @@ -32,6 +35,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 5 max_module_size: 1 @@ -40,6 +44,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 0 max_module_size: 1 @@ -48,6 +53,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 2 max_module_size: 1 @@ -56,6 +62,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: true priority: 1 max_module_size: 1 @@ -64,6 +71,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 4 max_module_size: 1 @@ -72,6 +80,7 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false + keep_last: false priority: 3 max_module_size: 1 @@ -80,5 +89,6 @@ enable_rtc: true enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true + keep_last: false priority: 7 max_module_size: 1 diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bea78664c65a3..216ba4ccebbc9 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -7,6 +7,7 @@ collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 th_moving_object_velocity: 1.0 + th_distance_to_middle_of_the_road: 0.1 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: false @@ -72,3 +73,65 @@ max_planning_time: 150.0 neighbor_radius: 8.0 margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon + hysteresis_factor_expand_rate: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 99dadc959f5a5..de5f66628f1c9 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,8 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. @@ -95,7 +95,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +105,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +118,11 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | ## **collision check** @@ -138,13 +132,14 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for occupancy grid based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid | [-] | bool | flag whether to use occupancy grid for collision check | true | -| use_occupancy_grid_for_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | ### **object recognition based collision check** @@ -174,12 +169,22 @@ searched for in certain range of the shoulder lane. | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -## **Path Generation** +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 2b98775e4fe81..0b0e01378b61a 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -59,16 +59,17 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | ## Safety check with static obstacles diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp new file mode 100644 index 0000000000000..82c387d04e525 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp @@ -0,0 +1,94 @@ +// 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. +#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ +#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include "std_msgs/msg/color_rgba.hpp" + +#include + +namespace marker_utils::colors +{ +using std_msgs::msg::ColorRGBA; + +inline ColorRGBA red(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0., 0., a); +} + +inline ColorRGBA green(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 1., 0., a); +} + +inline ColorRGBA blue(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 0., 1., a); +} + +inline ColorRGBA yellow(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 0., a); +} + +inline ColorRGBA aqua(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0., 1., 1., a); +} + +inline ColorRGBA magenta(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0., 1., a); +} + +inline ColorRGBA medium_orchid(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0.729, 0.333, 0.827, a); +} + +inline ColorRGBA light_pink(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 0.713, 0.756, a); +} + +inline ColorRGBA light_yellow(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 0.878, a); +} + +inline ColorRGBA light_steel_blue(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(0.690, 0.768, 0.870, a); +} + +inline ColorRGBA white(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(1., 1., 1., a); +} + +inline ColorRGBA grey(float a = 0.99) +{ + return tier4_autoware_utils::createMarkerColor(.5, .5, .5, a); +} + +inline std::vector colors_list(float a = 0.99) +{ + return {red(a), green(a), blue(a), yellow(a), aqua(a), + magenta(a), medium_orchid(a), light_pink(a), light_yellow(a), light_steel_blue(a)}; +} +} // namespace marker_utils::colors + +#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 6bcc9cee877b4..d337bae17ca6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -25,20 +25,9 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; -using marker_utils::CollisionCheckDebug; using visualization_msgs::msg::MarkerArray; -MarkerArray showObjectInfo( - const std::unordered_map & obj_debug_vec, std::string && ns); MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); -MarkerArray showLerpedPose( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showEgoPredictedPaths( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showPolygon( - const std::unordered_map & obj_debug_vec, std::string && ns); -MarkerArray showPolygonPose( - const std::unordered_map & obj_debug_vec, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 6894edaece828..c9a15bd5f42f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -29,7 +30,6 @@ #include #include -#include #include namespace marker_utils @@ -39,6 +39,9 @@ using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; @@ -49,50 +52,16 @@ using tier4_autoware_utils::Polygon2d; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -struct CollisionCheckDebug -{ - std::string failed_reason; - std::size_t lane_id{0}; - Pose current_pose{}; - Twist current_twist{}; - Twist object_twist{}; - Pose expected_ego_pose{}; - Pose expected_obj_pose{}; - Pose relative_to_ego{}; - double rss_longitudinal{0.0}; - double ego_to_obj_margin{0.0}; - double longitudinal_offset{0.0}; - double lateral_offset{0.0}; - bool is_front{false}; - bool allow_lane_change{false}; - std::vector lerped_path; - std::vector ego_predicted_path{}; - Polygon2d ego_polygon{}; - Polygon2d obj_polygon{}; -}; -using CollisionCheckDebugMap = std::unordered_map; - -constexpr std::array, 10> colorsList() -{ - constexpr std::array red = {1., 0., 0.}; - constexpr std::array green = {0., 1., 0.}; - constexpr std::array blue = {0., 0., 1.}; - constexpr std::array yellow = {1., 1., 0.}; - constexpr std::array aqua = {0., 1., 1.}; - constexpr std::array magenta = {1., 0., 1.}; - constexpr std::array medium_orchid = {0.729, 0.333, 0.827}; - constexpr std::array light_pink = {1, 0.713, 0.756}; - constexpr std::array light_yellow = {1, 1, 0.878}; - constexpr std::array light_steel_blue = {0.690, 0.768, 0.870}; - return {red, green, blue, yellow, aqua, - magenta, medium_orchid, light_pink, light_yellow, light_steel_blue}; -} - inline int64_t bitShift(int64_t original_id) { return original_id << (sizeof(int32_t) * 8 / 2); } +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); + +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); @@ -131,6 +100,15 @@ MarkerArray createObjectsMarkerArray( MarkerArray createDrivableLanesMarkerArray( const std::vector & drivable_lanes, std::string && ns); +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 1fdf4e020d71f..689a3db66a196 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -28,6 +28,7 @@ struct ModuleConfigParameters bool enable_rtc{false}; bool enable_simultaneous_execution_as_approved_module{false}; bool enable_simultaneous_execution_as_candidate_module{false}; + bool keep_last{false}; uint8_t priority{0}; uint8_t max_module_size{0}; }; @@ -142,20 +143,6 @@ struct BehaviorPathPlannerParameters // maximum drivable area visualization bool visualize_maximum_drivable_area; - - // collision check - double lateral_distance_max_threshold; - double longitudinal_distance_min_threshold; - double longitudinal_velocity_delta_time; - - double expected_front_deceleration; // brake parameter under normal lane change - double expected_rear_deceleration; // brake parameter under normal lane change - - double expected_front_deceleration_for_abort; // hard brake parameter for abort - double expected_rear_deceleration_for_abort; // hard brake parameter for abort - - double rear_vehicle_reaction_time; - double rear_vehicle_safety_time_margin; }; #endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ 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 8820ef70c37bf..6ca2ecf638da6 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 @@ -348,41 +348,21 @@ class PlannerManager candidate_module_ptrs_.clear(); } - /** - * @brief stop and remove not RUNNING modules in candidate_module_ptrs_. - */ - void clearNotRunningCandidateModules() - { - const auto it = std::remove_if( - candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [this](auto & m) { - if (m->getCurrentStatus() != ModuleStatus::RUNNING) { - deleteExpiredModules(m); - return true; - } - return false; - }); - candidate_module_ptrs_.erase(it, candidate_module_ptrs_.end()); - } - - /** - * @brief check if there is any RUNNING module in candidate_module_ptrs_. - */ - bool hasAnyRunningCandidateModule() - { - return std::any_of(candidate_module_ptrs_.begin(), candidate_module_ptrs_.end(), [](auto & m) { - return m->getCurrentStatus() == ModuleStatus::RUNNING; - }); - } - /** * @brief get current root lanelet. the lanelet is used for reference path generation. * @param planner data. * @return root lanelet. */ - lanelet::ConstLanelet updateRootLanelet(const std::shared_ptr & data) const + lanelet::ConstLanelet updateRootLanelet( + const std::shared_ptr & data, bool success_lane_change = false) const { lanelet::ConstLanelet ret{}; - data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + if (success_lane_change) { + data->route_handler->getClosestPreferredLaneletWithinRoute( + data->self_odometry->pose.pose, &ret); + } else { + data->route_handler->getClosestLaneletWithinRoute(data->self_odometry->pose.pose, &ret); + } RCLCPP_DEBUG(logger_, "update start lanelet. id:%ld", ret.id()); return ret; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 01c82e62c1cdc..9edb53fe8af63 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -223,6 +223,8 @@ class AvoidanceModule : public SceneModuleInterface */ void insertStopPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + void insertReturnDeadLine(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + /** * @brief insert stop point in output path. * @param target path. @@ -555,6 +557,8 @@ class AvoidanceModule : public SceneModuleInterface bool arrived_path_end_{false}; + bool safe_{true}; + std::shared_ptr parameters_; helper::avoidance::AvoidanceHelper helper_; @@ -575,6 +579,8 @@ class AvoidanceModule : public SceneModuleInterface ObjectDataArray registered_objects_; + mutable size_t safe_count_{0}; + mutable ObjectDataArray ego_stopped_objects_; mutable ObjectDataArray stopped_objects_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 5177feaaf61f4..2f465901a7085 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -24,6 +24,9 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -57,6 +60,12 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + enum class PathType { NONE = 0, SHIFT, @@ -65,18 +74,19 @@ enum class PathType { FREESPACE, }; -struct PUllOverStatus +struct PullOverStatus { std::shared_ptr pull_over_path{}; std::shared_ptr lane_parking_pull_over_path{}; size_t current_path_idx{0}; bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets pull_over_lanes{}; - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; - bool is_safe{false}; + lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain + lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain + std::vector lanes{}; // current + pull_over + bool has_decided_path{false}; // if true, the path is decided and safe against static objects + bool is_safe_static_objects{false}; // current path is safe against *static* objects + bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; @@ -127,10 +137,18 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; }; private: - PUllOverStatus status_; + PullOverStatus status_; + + mutable StartGoalPlannerData goal_planner_data_; std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; + + vehicle_info_util::VehicleInfo vehicle_info_; + // planner std::vector> pull_over_planners_; std::unique_ptr freespace_planner_; @@ -198,7 +216,6 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - boost::optional calcFeasibleDecelDistance(const double target_velocity) const; void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -267,6 +284,14 @@ class GoalPlannerModule : public SceneModuleInterface // rtc std::pair calcDistanceToPathChange() const; + // safety check + void initializeSafetyCheckParameters(); + SafetyCheckParams createSafetyCheckParams() const; + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index 30bbe39c8240f..4574536f9e771 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -40,6 +40,8 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void updateModuleParams(const std::vector & parameters) override; + bool isAlwaysExecutableModule() const override; + bool isSimultaneousExecutableAsApprovedModule() const override; bool isSimultaneousExecutableAsCandidateModule() const override; @@ -48,7 +50,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface std::shared_ptr parameters_; std::vector> registered_modules_; - bool left_side_parking_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 6ca06ff21c4a9..f30bf31d4e010 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -43,11 +43,11 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::LaneChangeDebugMsg; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 0b63d8e40ae81..d1476abb63e27 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -26,6 +26,8 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; @@ -33,8 +35,6 @@ using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygo using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; -using marker_utils::CollisionCheckDebugMap; using route_handler::Direction; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -140,8 +140,12 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, - std::unordered_map & debug_data) const; + const utils::path_safety_checker::RSSparams & rss_params, + CollisionCheckDebugMap & debug_data) const; + + LaneChangeTargetObjectIndices filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const; rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp index 00882866d6d2b..4ab09e0acd8f3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp @@ -54,6 +54,7 @@ class SceneModuleManagerInterface enable_simultaneous_execution_as_candidate_module_( config.enable_simultaneous_execution_as_candidate_module), enable_rtc_(config.enable_rtc), + keep_last_(config.keep_last), max_module_num_(config.max_module_size), priority_(config.priority) { @@ -210,16 +211,37 @@ class SceneModuleManagerInterface bool canLaunchNewModule() const { return registered_modules_.size() < max_module_num_; } + /** + * Determine if the module is always executable, regardless of the state of other modules. + * + * When this returns true: + * - The module can be executed even if other modules are not marked as 'simultaneously + * executable'. + * - Conversely, the presence of this module will not prevent other modules + * from executing, even if they are not marked as 'simultaneously executable'. + */ + virtual bool isAlwaysExecutableModule() const { return false; } + virtual bool isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_approved_module_; } virtual bool isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + return enable_simultaneous_execution_as_candidate_module_; } + bool isKeepLast() const { return keep_last_; } + void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } void reset() @@ -281,6 +303,8 @@ class SceneModuleManagerInterface private: bool enable_rtc_; + bool keep_last_; + size_t max_module_num_; size_t priority_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 24fd27d9a7a78..e332821607672 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -17,12 +17,15 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -43,6 +46,11 @@ namespace behavior_path_planner { +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; @@ -53,7 +61,8 @@ struct PullOutStatus PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; - bool is_safe{false}; + bool is_safe_static_objects{false}; // current path is safe against static objects + bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects bool back_finished{false}; Pose pull_out_start_pose{}; @@ -82,11 +91,19 @@ class StartPlannerModule : public SceneModuleInterface BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; + void processOnEntry() override; void processOnExit() override; void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; + if (parameters->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } void resetStatus(); @@ -100,11 +117,17 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: + void initializeSafetyCheckParameters(); + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; vehicle_info_util::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; + mutable StartGoalPlannerData start_planner_data_; std::deque odometry_buffer_; @@ -122,7 +145,8 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - std::vector searchPullOutStartPoses(); + PathWithLaneId calcStartPoseCandidatesBackwardPath() const; + std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; std::shared_ptr lane_departure_checker_; @@ -132,8 +156,8 @@ class StartPlannerModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; void planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority); + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; @@ -146,6 +170,10 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. @@ -154,6 +182,7 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); bool planFreespacePath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index ac6afde3ff47c..ec0ec4696a44d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -47,7 +48,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; -using marker_utils::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; struct ObjectParameter { @@ -192,14 +193,16 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double safety_check_time_horizon{0.0}; + double time_horizon_for_front_object{0.0}; + double time_horizon_for_rear_object{0.0}; double safety_check_time_resolution{0.0}; // find adjacent lane vehicles double safety_check_backward_distance; // transit hysteresis (unsafe to safe) - double safety_check_hysteresis_factor; + size_t hysteresis_factor_safe_count; + double hysteresis_factor_expand_rate; // don't output new candidate path if the offset between ego and path is larger than this. double safety_check_ego_offset; @@ -228,6 +231,9 @@ struct AvoidanceParameters // nominal avoidance sped double nominal_avoidance_speed; + // module try to return original path to keep this distance from edge point of the path. + double remain_buffer_distance; + // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. double soft_road_shoulder_margin{1.0}; @@ -302,6 +308,9 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // rss parameters + utils::path_safety_checker::RSSparams rss_params; + // clip left and right bounds for objects bool enable_bound_clipping{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 420ace9458220..ede0e5bfb728c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -88,7 +88,7 @@ std::vector generateObstaclePolygonsForDrivableArea( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters); + const bool is_object_front, const std::shared_ptr & parameters); double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index cb769a1f2884c..9cae5e18f1352 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -94,6 +94,10 @@ class GeometricParallelParking std::vector getArcPaths() const { return arc_paths_; } std::vector getPaths() const { return paths_; } + std::vector> getPairsTerminalVelocityAndAccel() const + { + return pairs_terminal_velocity_and_accel_; + } PathWithLaneId getPathByIdx(size_t const idx) const; PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; @@ -112,6 +116,7 @@ class GeometricParallelParking std::vector arc_paths_; std::vector paths_; + std::vector> pairs_terminal_velocity_and_accel_; size_t current_path_idx_ = 0; void clearPaths(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index f87944641f59b..4e40267ede4df 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -40,7 +41,6 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; @@ -59,8 +59,9 @@ struct GoalPlannerParameters double margin_from_boundary; // occupancy grid map - bool use_occupancy_grid; - bool use_occupancy_grid_for_longitudinal_margin; + bool use_occupancy_grid_for_goal_search; + bool use_occupancy_grid_for_goal_longitudinal_margin; + bool use_occupancy_grid_for_path_collision_check; double occupancy_grid_collision_check_margin; int theta_size; int obstacle_threshold; @@ -69,8 +70,10 @@ struct GoalPlannerParameters bool use_object_recognition; double object_recognition_collision_check_margin; double object_recognition_collision_check_max_extra_stopping_margin; + double th_moving_object_velocity; // pull over general params + double pull_over_minimum_request_length; double pull_over_velocity; double pull_over_minimum_velocity; double decide_path_distance; @@ -99,6 +102,18 @@ struct GoalPlannerParameters AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; + // debug bool print_debug_info; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4b800917d4aec..20788e53309ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using autoware_auto_planning_msgs::msg::PathWithLaneId; @@ -46,6 +47,8 @@ struct PullOverPath { PullOverPlannerType type{PullOverPlannerType::NONE}; std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; std::vector debug_poses{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 3dde96e813aad..0adda77a2ad72 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -46,15 +46,15 @@ using visualization_msgs::msg::MarkerArray; // TODO(sugahara) move to util PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side); +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance); PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking); -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 721c5b00f6e33..0556a780467c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -76,6 +76,10 @@ struct LaneChangeParameters bool check_motorcycle{true}; // check object motorbike bool check_pedestrian{true}; // check object pedestrian + // safety check + utils::path_safety_checker::RSSparams rss_params; + utils::path_safety_checker::RSSparams rss_params_for_abort; + // abort LaneChangeCancelParameters cancel; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 95bae3348818a..9eecc84838aad 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -183,12 +183,6 @@ boost::optional getLeadingStaticObjectIdx( std::optional createPolygon( const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist); -LaneChangeTargetObjectIndices filterObject( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, - const Pose & current_pose, const RouteHandler & route_handler, - const LaneChangeParameters & lane_change_parameters); - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index b4575eb4d3b7e..8a3b7094c69d1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -55,24 +55,37 @@ PredictedObjects filterObjects( const std::shared_ptr & params); /** - * @brief Filter objects based on their velocity. + * @brief Filters objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param lim_v Velocity limit for filtering. - * @return PredictedObjects The filtered objects. + * Depending on the remove_above_threshold parameter, this function either removes objects with + * velocities above the given threshold or only keeps those objects. It uses the helper function + * filterObjectsByVelocity() to do the actual filtering. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The velocity threshold for the filtering. + * @param remove_above_threshold If true, objects with velocities above the threshold are removed. + * If false, only objects with velocities above the threshold are + * kept. + * @return A new collection of objects that have been filtered according to the rules. */ -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v); +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold = true); /** - * @brief Filter objects based on a velocity range. + * @brief Helper function to filter objects based on their velocity. * - * @param objects The predicted objects to filter. - * @param min_v Minimum velocity for filtering. - * @param max_v Maximum velocity for filtering. - * @return PredictedObjects The filtered objects. + * This function iterates over all objects and calculates their velocity norm. If the velocity norm + * is within the velocity_threshold and max_velocity range, the object is added to a new collection. + * This new collection is then returned. + * + * @param objects The objects to be filtered. + * @param velocity_threshold The minimum velocity for an object to be included in the output. + * @param max_velocity The maximum velocity for an object to be included in the output. + * @return A new collection of objects that have been filtered according to the rules. */ PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v); + const PredictedObjects & objects, double velocity_threshold, double max_velocity); /** * @brief Filter objects based on their position relative to a current_pose. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 519f589f561ee..b2433c076b1a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -19,13 +19,18 @@ #include #include +#include +#include +#include +#include #include namespace behavior_path_planner::utils::path_safety_checker { using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; struct PoseWithVelocity @@ -123,6 +128,8 @@ struct RSSparams double longitudinal_distance_min_threshold{ 0.0}; ///< Minimum threshold for longitudinal distance. double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. + double front_vehicle_deceleration; ///< brake parameter + double rear_vehicle_deceleration; ///< brake parameter }; /** @@ -162,13 +169,38 @@ struct ObjectsFilteringParams */ struct SafetyCheckParams { - bool enable_safety_check; ///< Enable safety checks. - double backward_lane_length; ///< Length of the backward lane for path generation. + bool enable_safety_check; ///< Enable safety checks. + double + hysteresis_factor_expand_rate; ///< Hysteresis factor to expand/shrink polygon with the value. + double backward_path_length; ///< Length of the backward lane for path generation. double forward_path_length; ///< Length of the forward path lane for path generation. RSSparams rss_params; ///< Parameters related to the RSS model. bool publish_debug_marker{false}; ///< Option to publish debug markers. }; +struct CollisionCheckDebug +{ + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. + double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. + std::vector ego_predicted_path; ///< ego vehicle's predicted path. + std::vector obj_predicted_path; ///< object's predicted path. + Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. + Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. +}; +using CollisionCheckDebugPair = std::pair; +using CollisionCheckDebugMap = + std::unordered_map; + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8606f2cd2397d..a9404038d831a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -43,15 +43,18 @@ namespace behavior_path_planner::utils::path_safety_checker using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::Shape; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; namespace bg = boost::geometry; +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); @@ -63,10 +66,12 @@ Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, CollisionCheckDebug & debug); +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution); + double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, @@ -98,8 +103,8 @@ bool checkCollision( const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug); + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + const double hysteresis_factor, CollisionCheckDebug & debug); /** * @brief Check collision between ego path footprints with extra longitudinal stopping margin and diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp new file mode 100644 index 0000000000000..4ba2cb08b6341 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -0,0 +1,45 @@ +// 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. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ + +#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include + +#include + +namespace behavior_path_planner +{ +using autoware_auto_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +/* + * Common data for start/goal_planner module + */ +struct StartGoalPlannerData +{ + // filtered objects + PredictedObjects filtered_objects; + TargetObjectsOnLane target_objects_on_lane; + std::vector ego_predicted_path; +}; + +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp new file mode 100644 index 0000000000000..bea671c266899 --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -0,0 +1,94 @@ +// 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. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ + +#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" +#include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" + +#include + +#include +#include +#include + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using behavior_path_planner::StartPlannerParameters; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity); + +/** + * @brief Update path velocities based on driving direction. + * + * This function updates the longitudinal velocity of each point in the provided paths, + * based on whether the vehicle is driving forward or backward. It also sets the terminal + * velocity and acceleration for each path. + * + * @param paths A vector of paths with lane IDs to be updated. + * @param terminal_vel_acc_pairs A vector of pairs, where each pair contains the terminal + * velocity and acceleration for a corresponding path. + * @param target_velocity The target velocity for ego vehicle predicted path. + * @param acceleration The acceleration for ego vehicle predicted path. + */ +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params); + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params); + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params); + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params); + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel); + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx); + +} // namespace behavior_path_planner::utils::start_goal_planner_common + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index abdf17c9c6cfe..2b2de183b2dac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() override { return PlannerType::FREESPACE; } - boost::optional plan(Pose start_pose, Pose end_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 7ace770dc7ff1..a5ff52e74038f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp index d68985ec27350..608b918e839fd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp @@ -19,6 +19,7 @@ #include +#include #include namespace behavior_path_planner @@ -27,6 +28,8 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; + // accelerate with constant acceleration to the target velocity + std::vector> pairs_terminal_velocity_and_accel{}; Pose start_pose{}; Pose end_pose{}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 6e6e5111e5dd9..cb662efd767bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -60,7 +60,7 @@ class PullOutPlannerBase } virtual PlannerType getPlannerType() = 0; - virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 2042593064c51..0842d0a17bd97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 1e26bef0c85be..9e9173c3917a0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -17,6 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -38,6 +39,7 @@ struct StartPlannerParameters double th_stopped_velocity; double th_stopped_time; double th_turn_signal_on_lateral_offset; + double th_distance_to_middle_of_the_road; double intersection_search_length; double length_ratio_for_turn_signal_deactivation_near_intersection; double collision_check_margin; @@ -76,6 +78,18 @@ struct StartPlannerParameters PlannerCommonParam freespace_planner_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; + + // stop condition + double maximum_deceleration_for_stop; + double maximum_jerk_for_stop; + + // hysteresis parameter + double hysteresis_factor_expand_rate; + + // path safety checker + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; + utils::path_safety_checker::SafetyCheckParams safety_check_params; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 63e374e074a5a..d85e6181764d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -16,6 +16,9 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -36,6 +39,7 @@ namespace behavior_path_planner::start_planner_utils 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::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 5ab03c34e7e14..8c700dfb1d7af 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -320,8 +320,8 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side); // misc @@ -412,6 +412,22 @@ std::optional getPolygonByPoint( lanelet::ConstLanelets combineLanelets( const lanelet::ConstLanelets & base_lanes, const lanelet::ConstLanelets & added_lanes); + +/** + * @brief removeInverseOrderPathPoints function + * + * This function is designed to handle a situation that can arise when shifting paths on a curve, + * where the positions of the path points may become inverted (i.e., a point further along the path + * comes to be located before an earlier point). It corrects for this by using the function + * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in + * the correct order (with the second point being 'forward' of the first). Any points which fail + * this test are omitted from the returned PathWithLaneId. + * + * @param path A path with points possibly in incorrect order. + * @return Returns a new PathWithLaneId that has all points in the correct order. + */ +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path); + } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 29f1ee762abdf..3f33bcd7b64dc 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -66,7 +66,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_publisher_ = create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); - modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); + const auto durable_qos = rclcpp::QoS(1).transient_local(); + modified_goal_publisher_ = + create_publisher("~/output/modified_goal", durable_qos); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); @@ -282,6 +284,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter(ns + "enable_simultaneous_execution_as_approved_module"); config.enable_simultaneous_execution_as_candidate_module = declare_parameter(ns + "enable_simultaneous_execution_as_candidate_module"); + config.keep_last = declare_parameter(ns + "keep_last"); config.priority = declare_parameter(ns + "priority"); config.max_module_size = declare_parameter(ns + "max_module_size"); return config; @@ -391,23 +394,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - p.lateral_distance_max_threshold = declare_parameter("lateral_distance_max_threshold"); - p.longitudinal_distance_min_threshold = - declare_parameter("longitudinal_distance_min_threshold"); - p.longitudinal_velocity_delta_time = - declare_parameter("longitudinal_velocity_delta_time"); - - p.expected_front_deceleration = declare_parameter("expected_front_deceleration"); - p.expected_rear_deceleration = declare_parameter("expected_rear_deceleration"); - - p.expected_front_deceleration_for_abort = - declare_parameter("expected_front_deceleration_for_abort"); - p.expected_rear_deceleration_for_abort = - declare_parameter("expected_rear_deceleration_for_abort"); - - p.rear_vehicle_reaction_time = declare_parameter("rear_vehicle_reaction_time"); - p.rear_vehicle_safety_time_margin = declare_parameter("rear_vehicle_safety_time_margin"); - if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index fca53930a0ec5..7493982db78b1 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" @@ -33,43 +34,8 @@ namespace marker_utils::lane_change_markers { using geometry_msgs::msg::Point; using tier4_autoware_utils::createDefaultMarker; -using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; -MarkerArray showObjectInfo( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - Marker obj_marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999)); - - MarkerArray marker_array; - int32_t id{0}; - - marker_array.markers.reserve(obj_debug_vec.size()); - - int idx{0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - obj_marker.id = ++id; - obj_marker.pose = info.current_pose; - - std::ostringstream ss; - - ss << "Idx: " << ++idx << "\nReason: " << info.failed_reason - << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal - << "\nEgo to obj: " << info.ego_to_obj_margin << "\nLateral offset: " << info.lateral_offset - << "\nLongitudinal offset: " << info.longitudinal_offset - << "\nPosition: " << (info.is_front ? "front" : "back") - << "\nSafe: " << (info.allow_lane_change ? "Yes" : "No"); - - obj_marker.text = ss.str(); - - marker_array.markers.push_back(obj_marker); - } - return marker_array; -} - MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) { if (lanes.empty()) { @@ -80,7 +46,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes int32_t id{0}; const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - constexpr auto colors = colorsList(); + const auto colors = colors::colors_list(); const auto loop_size = std::min(lanes.size(), colors.size()); marker_array.markers.reserve(loop_size); @@ -91,8 +57,7 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes const auto & color = colors.at(idx); Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), - createMarkerColor(color[0], color[1], color[2], 0.9)); + "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color); marker.points.reserve(lanes.at(idx).path.points.size()); for (const auto & point : lanes.at(idx).path.points) { @@ -104,165 +69,6 @@ MarkerArray showAllValidLaneChangePath(const std::vector & lanes return marker_array; } -MarkerArray showLerpedPose( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - MarkerArray marker_array; - int32_t id{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.3, 0.3, 0.3), - createMarkerColor(1.0, 0.3, 1.0, 0.9)); - marker.points.reserve(info.lerped_path.size()); - - for (const auto & point : info.lerped_path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - return marker_array; -} - -MarkerArray showEgoPredictedPaths( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - MarkerArray marker_array; - constexpr auto colors = colorsList(); - constexpr float scale_val = 0.2; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - int32_t id{0}; - for (const auto & [uuid, info] : obj_debug_vec) { - const auto loop_size = std::min(info.ego_predicted_path.size(), colors.size()); - - for (std::size_t idx = 0; idx < loop_size; ++idx) { - const auto & path = info.ego_predicted_path.at(idx).path; - const auto & color = colors.at(idx); - - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, - createMarkerScale(scale_val, scale_val, scale_val), - createMarkerColor(color[0], color[1], color[2], 0.9)); - - marker.points.reserve(path.size()); - - for (const auto & point : path) { - marker.points.push_back(point.position); - } - - marker_array.markers.push_back(marker); - } - } - return marker_array; -} - -MarkerArray showPolygon( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - if (obj_debug_vec.empty()) { - return MarkerArray{}; - } - - constexpr float scale_val{0.2}; - int32_t id{0}; - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); - Marker ego_marker = createDefaultMarker( - "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), - createMarkerColor(1.0, 1.0, 1.0, 0.9)); - Marker obj_marker = ego_marker; - - auto text_marker = createDefaultMarker( - "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 1.0)); - - MarkerArray marker_array; - - constexpr auto red_color = colorsList().at(0); - constexpr auto green_color = colorsList().at(1); - const auto reserve_size = obj_debug_vec.size(); - - marker_array.markers.reserve(reserve_size * 4); - - int32_t idx = {0}; - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = info.allow_lane_change ? green_color : red_color; - const auto & ego_polygon = info.ego_polygon.outer(); - const auto poly_z = info.current_pose.position.z; // temporally - ego_marker.id = ++id; - ego_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); - ego_marker.points.reserve(ego_polygon.size()); - for (const auto & p : ego_polygon) { - ego_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(ego_marker); - - std::ostringstream ss; - text_marker.id = ego_marker.id; - ss << ++idx; - text_marker.text = ss.str(); - text_marker.pose = info.expected_ego_pose; - - marker_array.markers.push_back(text_marker); - - const auto & obj_polygon = info.obj_polygon.outer(); - obj_marker.id = ++id; - obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); - obj_marker.points.reserve(obj_polygon.size()); - for (const auto & p : obj_polygon) { - obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); - } - marker_array.markers.push_back(obj_marker); - - text_marker.id = obj_marker.id; - text_marker.pose = info.expected_obj_pose; - marker_array.markers.push_back(text_marker); - - ego_marker.points.clear(); - obj_marker.points.clear(); - } - - return marker_array; -} - -MarkerArray showPolygonPose( - const std::unordered_map & obj_debug_vec, std::string && ns) -{ - constexpr auto colors = colorsList(); - const auto loop_size = std::min(colors.size(), obj_debug_vec.size()); - MarkerArray marker_array; - int32_t id{0}; - size_t idx{0}; - const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; - marker_array.markers.reserve(obj_debug_vec.size()); - - for (const auto & [uuid, info] : obj_debug_vec) { - const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::POINTS, createMarkerScale(0.2, 0.2, 0.2), - createMarkerColor(color[0], color[1], color[2], 0.999)); - marker.points.reserve(2); - marker.points.push_back(info.expected_ego_pose.position); - marker.points.push_back(info.expected_obj_pose.position); - marker_array.markers.push_back(marker); - ++idx; - if (idx >= loop_size) { - break; - } - } - - return marker_array; -} - MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns) @@ -273,7 +79,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto wall_marker = createDefaultMarker( "map", now, ns + "virtual_wall", id, visualization_msgs::msg::Marker::CUBE, - createMarkerScale(0.1, 5.0, 2.0), createMarkerColor(0.0, 1.0, 0.0, 0.5)); + createMarkerScale(0.1, 5.0, 2.0), colors::green()); wall_marker.pose = lane_changing_pose; wall_marker.pose.position.z += 1.0; marker_array.markers.push_back(wall_marker); @@ -282,7 +88,7 @@ MarkerArray createLaneChangingVirtualWallMarker( { auto text_marker = createDefaultMarker( "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0), colors::white()); text_marker.pose = lane_changing_pose; text_marker.pose.position.z += 2.0; text_marker.text = module_name; diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 80fea95b03caa..aac4d9235146c 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -14,6 +14,8 @@ #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/marker_utils/colors.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -23,6 +25,7 @@ namespace marker_utils { using behavior_path_planner::ShiftLine; using behavior_path_planner::utils::calcPathArcLengthArray; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; @@ -32,6 +35,27 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) +{ + CollisionCheckDebug debug; + debug.current_obj_pose = obj.initial_pose.pose; + debug.current_twist = obj.initial_twist.twist; + return {tier4_autoware_utils::toHexString(obj.uuid), debug}; +} + +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe) +{ + auto & [key, element] = object_debug; + element.is_safe = is_safe; + if (debug_map.find(key) != debug_map.end()) { + debug_map[key] = element; + return; + } + + debug_map.insert(object_debug); +} + MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) @@ -408,4 +432,217 @@ MarkerArray createDrivableLanesMarkerArray( return msg; } +MarkerArray createPredictedPathMarkerArray( + const PredictedPath & predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, + std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + if (predicted_path.path.empty()) { + return MarkerArray{}; + } + + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + const auto & path = predicted_path.path; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.1), + + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + for (size_t i = 0; i < path.size(); ++i) { + marker.id = i + id; + marker.points.clear(); + + const auto & predicted_path_pose = path.at(i); + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, -half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, base_to_front, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, half_width, 0.0) + .position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(predicted_path_pose, -base_to_rear, -half_width, 0.0) + .position); + marker.points.push_back(marker.points.front()); + + marker_array.markers.push_back(marker); + } + return marker_array; + + marker.points.reserve(path.size()); + for (const auto & point : path) { + marker.points.push_back(point.position); + } + msg.markers.push_back(marker); + return msg; +} + +MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + if (obj_debug_vec.empty()) { + return MarkerArray{}; + } + + int32_t id{0}; + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + constexpr float line_scale_val{0.2}; + const auto line_marker_scale = createMarkerScale(line_scale_val, line_scale_val, line_scale_val); + + auto default_line_marker = [&](const auto & color = colors::green()) { + return createDefaultMarker("map", now, ns, ++id, Marker::LINE_STRIP, line_marker_scale, color); + }; + + constexpr float text_scale_val{1.5}; + const auto text_marker_scale = createMarkerScale(text_scale_val, text_scale_val, text_scale_val); + + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", now, ns + "_text", ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + text_marker_scale, colors::white()); + }; + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + obj_debug_vec.size() * 5); // poly ego, text ego, poly obj, text obj, cube obj + + int32_t idx = {0}; + for (const auto & [uuid, info] : obj_debug_vec) { + const auto color = info.is_safe ? colors::green() : colors::red(); + const auto poly_z = info.current_obj_pose.position.z; // temporally + + const auto insert_polygon_marker = [&](const auto & polygon) { + marker_array.markers.emplace_back(); + auto & polygon_marker = marker_array.markers.back(); + polygon_marker = default_line_marker(color); + polygon_marker.points.reserve(polygon.outer().size()); + for (const auto & p : polygon.outer()) { + polygon_marker.points.push_back(createPoint(p.x(), p.y(), poly_z)); + } + }; + + insert_polygon_marker(info.extended_ego_polygon); + insert_polygon_marker(info.extended_obj_polygon); + + const auto str_idx = std::to_string(++idx); + const auto insert_text_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & text_marker = marker_array.markers.back(); + text_marker = default_text_marker(); + text_marker.text = str_idx; + text_marker.pose = pose; + }; + + insert_text_marker(info.expected_ego_pose); + insert_text_marker(info.expected_obj_pose); + + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(info.current_obj_pose); + } + return marker_array; +} + +MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; + const auto arrow_marker_scale = createMarkerScale(1.0, 0.3, 0.3); + const auto default_arrow_marker = [&](const auto & color) { + return createDefaultMarker( + "map", current_time, ns, ++id, Marker::ARROW, arrow_marker_scale, color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve(std::accumulate( + obj_debug_vec.cbegin(), obj_debug_vec.cend(), 0UL, + [&](const auto current_sum, const auto & obj_debug) { + const auto & [uuid, info] = obj_debug; + return current_sum + info.ego_predicted_path.size() + info.obj_predicted_path.size() + 2; + })); + + for (const auto & [uuid, info] : obj_debug_vec) { + const auto insert_marker = [&](const auto & path, const auto & color) { + for (const auto & pose : path) { + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose.pose; + } + }; + + insert_marker(info.ego_predicted_path, colors::aqua()); + insert_marker(info.obj_predicted_path, colors::yellow()); + const auto insert_expected_pose_marker = [&](const auto & pose, const auto & color) { + // instead of checking for distance, inserting a new marker might be more efficient + marker_array.markers.emplace_back(); + auto & marker = marker_array.markers.back(); + marker = default_arrow_marker(color); + marker.pose = pose; + marker.pose.position.z += 0.05; + }; + + insert_expected_pose_marker(info.expected_ego_pose, colors::red()); + insert_expected_pose_marker(info.expected_obj_pose, colors::red()); + } + return marker_array; +} + +MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns) +{ + int32_t id{0}; + auto default_text_marker = [&]() { + return createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, ++id, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), colors::aqua()); + }; + + MarkerArray marker_array; + + marker_array.markers.reserve(obj_debug_vec.size()); + + int idx{0}; + + for (const auto & [uuid, info] : obj_debug_vec) { + auto safety_check_info_text = default_text_marker(); + safety_check_info_text.pose = info.current_obj_pose; + + std::ostringstream ss; + + ss << "Idx: " << ++idx << "\nUnsafe reason: " << info.unsafe_reason + << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal + << "\nEgo to obj: " << info.inter_vehicle_distance + << "\nExtended polygon: " << (info.is_front ? "ego" : "object") + << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset + << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") + << "\nSafe: " << (info.is_safe ? "Yes" : "No"); + + safety_check_info_text.text = ss.str(); + marker_array.markers.push_back(safety_check_info_text); + } + return marker_array; +} + } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a3309968dd62c..d3ee7bcc093ec 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -188,22 +188,6 @@ std::vector PlannerManager::getRequestModules( std::vector request_modules{}; - /** - * check whether it is possible to push back more modules to approved modules. - */ - { - const auto find_block_module = [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsApprovedModule(); - }; - - const auto itr = - std::find_if(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); - - if (itr != approved_module_ptrs_.end()) { - return {}; - } - } - const auto toc = [this](const auto & name) { processing_time_.at(name) += stop_watch_.toc(name, true); }; @@ -212,14 +196,62 @@ std::vector PlannerManager::getRequestModules( stop_watch_.tic(manager_ptr->getModuleName()); /** - * don't launch candidate module if approved modules already exist. + * determine the execution capability of modules based on existing approved modules. */ - if (!approved_module_ptrs_.empty()) { - if (!manager_ptr->isSimultaneousExecutableAsApprovedModule()) { - toc(manager_ptr->getModuleName()); + // Condition 1: always executable module can be added regardless of the existence of other + // modules, so skip checking the existence of other modules. + // in other cases, need to check the existence of other modules and which module can be added. + const bool has_non_always_executable_module = std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) { + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 2: do not add modules that are neither always nor simultaneous executable + // if there exists at least one approved module that is simultaneous but not always + // executable. (only modules that are either always executable or simultaneous executable can + // be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return manager_ptr->isSimultaneousExecutableAsApprovedModule(); }}); + + // Condition 3: do not add modules that are not always executable if there exists + // at least one approved module that is neither always nor simultaneous executable. + // (only modules that are always executable can be added) + conditions.push_back( + {[&](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsApprovedModule(); + }, + [&]() { return false; }}); + + bool skip_module = false; + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = std::find_if( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), find_block_module); + + if (itr != approved_module_ptrs_.end() && !is_executable()) { + toc(manager_ptr->getModuleName()); + skip_module = true; + continue; + } + } + if (skip_module) { continue; } } + // else{ + // Condition 4: if none of the above conditions are met, any module can be added. + // (when the approved modules are either empty or consist only of always executable modules.) + // } /** * launch new candidate module. @@ -342,20 +374,58 @@ std::pair PlannerManager::runRequestModule * remove non-executable modules. */ for (const auto & module_ptr : sorted_request_modules) { - if (!getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule()) { - if (executable_modules.empty()) { - executable_modules.push_back(module_ptr); - break; - } + // Condition 1: always executable module can be added regardless of the existence of other + // modules. + if (getManager(module_ptr)->isAlwaysExecutableModule()) { + executable_modules.push_back(module_ptr); + continue; } - const auto itr = - std::find_if(executable_modules.begin(), executable_modules.end(), [this](const auto & m) { - return !getManager(m)->isSimultaneousExecutableAsCandidateModule(); - }); - - if (itr == executable_modules.end()) { + // Condition 4: If the executable modules are either empty or consist only of always executable + // modules, any module can be added. + const bool has_non_always_executable_module = std::any_of( + executable_modules.begin(), executable_modules.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + if (!has_non_always_executable_module) { executable_modules.push_back(module_ptr); + continue; + } + + // pairs of find_block_module and is_executable + std::vector, std::function>> + conditions; + + // Condition 3: Only modules that are always executable can be added + // if there exists at least one executable module that is neither always nor simultaneous + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + !getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return false; }}); + + // Condition 2: Only modules that are either always executable or simultaneous executable can be + // added if there exists at least one executable module that is simultaneous but not always + // executable. + conditions.push_back( + {[this](const SceneModulePtr & m) { + return !getManager(m)->isAlwaysExecutableModule() && + getManager(m)->isSimultaneousExecutableAsCandidateModule(); + }, + [&]() { return getManager(module_ptr)->isSimultaneousExecutableAsCandidateModule(); }}); + + for (const auto & condition : conditions) { + const auto & find_block_module = condition.first; + const auto & is_executable = condition.second; + + const auto itr = + std::find_if(executable_modules.begin(), executable_modules.end(), find_block_module); + + if (itr != executable_modules.end() && is_executable()) { + executable_modules.push_back(module_ptr); + break; + } } } @@ -439,12 +509,21 @@ std::pair PlannerManager::runRequestModule BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptr & data) { - const bool has_any_running_candidate_module = hasAnyRunningCandidateModule(); - std::unordered_map results; BehaviorModuleOutput output = getReferencePath(data); results.emplace("root", output); + if (approved_module_ptrs_.empty()) { + return output; + } + + // move modules whose keep last flag is true to end of the approved_module_ptrs_. + { + std::sort(approved_module_ptrs_.begin(), approved_module_ptrs_.end(), [this](auto a, auto b) { + return !getManager(a)->isKeepLast() && getManager(b)->isKeepLast(); + }); + } + /** * execute all approved modules. */ @@ -461,22 +540,32 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisWaitingApproval(); }; + const auto not_keep_last_module = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [this](const auto & m) { return !getManager(m)->isKeepLast(); }); - const auto itr = std::find_if( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), waiting_approval_modules); + // convert reverse iterator -> iterator + const auto begin_itr = not_keep_last_module != approved_module_ptrs_.rend() + ? std::next(not_keep_last_module).base() + : approved_module_ptrs_.begin(); - if (itr != approved_module_ptrs_.end()) { + const auto waiting_approval_modules_itr = std::find_if( + begin_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->isWaitingApproval(); }); + + if (waiting_approval_modules_itr != approved_module_ptrs_.end()) { clearCandidateModules(); - candidate_module_ptrs_.push_back(*itr); + candidate_module_ptrs_.push_back(*waiting_approval_modules_itr); + + debug_info_.emplace_back( + *waiting_approval_modules_itr, Action::MOVE, "Back To Waiting Approval"); std::for_each( - std::next(itr), approved_module_ptrs_.end(), [this](auto & m) { deleteExpiredModules(m); }); + waiting_approval_modules_itr, approved_module_ptrs_.end(), + [&results](const auto & m) { results.erase(m->name()); }); - debug_info_.emplace_back(*itr, Action::MOVE, "Back To Waiting Approval"); + approved_module_ptrs_.erase(waiting_approval_modules_itr); } - - approved_module_ptrs_.erase(itr, approved_module_ptrs_.end()); } /** @@ -506,67 +595,39 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrname(); - const auto approved_modules_output = [&output_module_name, &results]() { - if (results.count(output_module_name) == 0) { - return results.at("root"); + const auto approved_modules_output = [&results, this]() { + const auto itr = std::find_if( + approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), + [&results](const auto & m) { return results.count(m->name()) != 0; }); + + if (itr != approved_module_ptrs_.rend()) { + return results.at((*itr)->name()); } - return results.at(output_module_name); + return results.at("root"); }(); - const auto not_success_itr = std::find_if( - approved_module_ptrs_.rbegin(), approved_module_ptrs_.rend(), - [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); - - // convert reverse iterator -> iterator - const auto success_itr = std::prev(not_success_itr).base() - 1; - - /** - * there is no succeeded module. return. - */ - if (success_itr == approved_module_ptrs_.end()) { - return approved_modules_output; - } - - const auto lane_change_itr = std::find_if( - success_itr, approved_module_ptrs_.end(), - [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - /** - * remove success modules according to Last In First Out(LIFO) policy. when the next module is in - * ModuleStatus::RUNNING, the previous module keeps running even if it is in - * ModuleStatus::SUCCESS. + * remove success module immediately. if lane change module has succeeded, update root lanelet. */ - if (lane_change_itr == approved_module_ptrs_.end() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { - debug_info_.emplace_back(m, Action::DELETE, "From Approved"); - deleteExpiredModules(m); - }); + { + const auto success_module_itr = std::partition( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [](const auto & m) { return m->getCurrentStatus() != ModuleStatus::SUCCESS; }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); + const auto success_lane_change = std::any_of( + success_module_itr, approved_module_ptrs_.end(), + [](const auto & m) { return m->name().find("lane_change") != std::string::npos; }); - return approved_modules_output; - } + if (success_lane_change) { + root_lanelet_ = updateRootLanelet(data, true); + } - /** - * as an exception, when there is lane change module is in succeeded modules, it doesn't remove - * any modules if module whose status is NOT ModuleStatus::SUCCESS exists. this is because the - * root lanelet is updated at the moment of lane change module's unregistering, and that causes - * change First In module's input. - */ - if (not_success_itr == approved_module_ptrs_.rend() && !has_any_running_candidate_module) { - std::for_each(success_itr, approved_module_ptrs_.end(), [this](auto & m) { + std::for_each(success_module_itr, approved_module_ptrs_.end(), [&](auto & m) { debug_info_.emplace_back(m, Action::DELETE, "From Approved"); deleteExpiredModules(m); }); - approved_module_ptrs_.erase(success_itr, approved_module_ptrs_.end()); - clearNotRunningCandidateModules(); - - root_lanelet_ = updateRootLanelet(data); - - return approved_modules_output; + approved_module_ptrs_.erase(success_module_itr, approved_module_ptrs_.end()); } return approved_modules_output; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 66808be897ae0..8f2e505fcb76e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -45,7 +45,7 @@ namespace behavior_path_planner { -using marker_utils::CollisionCheckDebug; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -146,7 +146,10 @@ ModuleStatus AvoidanceModule::updateState() { const auto & data = avoidance_data_; const auto is_plan_running = isAvoidancePlanRunning(); - const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_avoidance_target = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "previous module lane is updated."); @@ -670,6 +673,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif throw std::domain_error("invalid behavior"); } + insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); + setStopReason(StopReason::AVOIDANCE, path.path); } @@ -1013,9 +1018,10 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; // The end_margin also has the purpose of preventing the return path from NOT being // triggered at the end point. - const auto end_margin = 1.0; - const auto return_remaining_distance = - std::max(data.arclength_from_ego.back() - o.longitudinal - offset - end_margin, 0.0); + const auto return_remaining_distance = std::max( + data.arclength_from_ego.back() - o.longitudinal - offset - + parameters_->remain_buffer_distance, + 0.0); al_return.start_shift_length = feasible_shift_length.get(); al_return.end_shift_length = 0.0; @@ -1795,14 +1801,14 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) return; } - const auto remaining_distance = arclength_from_ego.back(); + const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = avoidance_data_.arclength_from_ego.at(last_sl.end_idx); // check if there is enough distance for return. - if (last_sl_distance + 1.0 > remaining_distance) { // tmp: add some small number (+1.0) - DEBUG_PRINT("No enough distance for return."); + if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); return; } @@ -1891,8 +1897,10 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const auto ego_predicted_path = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, parameters_); + const auto ego_predicted_path_for_front_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, true, parameters_); + const auto ego_predicted_path_for_rear_object = + utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, false, parameters_); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { @@ -1915,23 +1923,42 @@ bool AvoidanceModule::isSafePath( return true; } + const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; + const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( avoidance_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { + const auto obj_polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + + const auto is_object_front = + utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); + + const auto is_object_incoming = + std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2; + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); + + const auto & ego_predicted_path = is_object_front && !is_object_incoming + ? ego_predicted_path_for_front_object + : ego_predicted_path_for_rear_object; + for (const auto & obj_path : obj_predicted_paths) { CollisionCheckDebug collision{}; if (!utils::path_safety_checker::checkCollision( - shifted_path.path, ego_predicted_path, object, obj_path, p, - p.expected_front_deceleration, p.expected_rear_deceleration, collision)) { + shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, + hysteresis_factor, collision)) { + safe_count_ = 0; return false; } } } - return true; + safe_count_++; + + return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const @@ -2577,6 +2604,8 @@ void AvoidanceModule::updateData() fillShiftLine(avoidance_data_, debug_data_); fillEgoStatus(avoidance_data_, debug_data_); fillDebugData(avoidance_data_, debug_data_); + + safe_ = avoidance_data_.safe; } void AvoidanceModule::processOnEntry() @@ -2906,6 +2935,77 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } +void AvoidanceModule::insertReturnDeadLine( + const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +{ + const auto & data = avoidance_data_; + + if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) { + RCLCPP_DEBUG(getLogger(), "goal is far enough."); + return; + } + + const auto shift_length = path_shifter_.getLastShiftLength(); + + if (std::abs(shift_length) < 1e-3) { + RCLCPP_DEBUG(getLogger(), "don't have to consider return shift."); + return; + } + + const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); + + const auto to_goal = calcSignedArcLength( + shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); + const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + + // If we don't need to consider deceleration constraints, insert a deceleration point + // and return immediately + if (!use_constraints_for_decel) { + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, + stop_pose_); + return; + } + + // If the stop distance is not enough for comfortable stop, don't insert wait point. + const auto is_comfortable_stop = helper_.getFeasibleDecelDistance(0.0) < to_stop_line; + if (!is_comfortable_stop) { + RCLCPP_DEBUG(getLogger(), "stop distance is not enough."); + return; + } + + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); + + // insert slow down speed. + const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), to_stop_line); + if (current_target_velocity < getEgoSpeed()) { + RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; + if (shift_longitudinal_distance < 0.0) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), shift_longitudinal_distance); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + const double v_insert = + std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed); + + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); + } +} + void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { @@ -3036,6 +3136,12 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto object = data.target_objects.front(); + const auto enough_space = + object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + if (!enough_space) { + return; + } + // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index a039c0388b6a4..14f98d026c493 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -136,7 +136,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.object_last_seen_threshold = get_parameter(node, ns + "object_last_seen_threshold"); } - // safety check + // safety check general params { std::string ns = "avoidance.safety_check."; p.enable_safety_check = get_parameter(node, ns + "enable"); @@ -146,13 +146,36 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_unavoidable_object = get_parameter(node, ns + "check_unavoidable_object"); p.check_other_object = get_parameter(node, ns + "check_other_object"); p.check_all_predicted_path = get_parameter(node, ns + "check_all_predicted_path"); - p.safety_check_time_horizon = get_parameter(node, ns + "time_horizon"); + p.time_horizon_for_front_object = + get_parameter(node, ns + "time_horizon_for_front_object"); + p.time_horizon_for_rear_object = + get_parameter(node, ns + "time_horizon_for_rear_object"); p.safety_check_time_resolution = get_parameter(node, ns + "time_resolution"); p.safety_check_backward_distance = get_parameter(node, ns + "safety_check_backward_distance"); - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); + p.hysteresis_factor_safe_count = get_parameter(node, ns + "hysteresis_factor_safe_count"); + } + + // safety check rss params + { + std::string ns = "avoidance.safety_check."; + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, ns + "longitudinal_distance_min_threshold"); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, ns + "longitudinal_velocity_delta_time"); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, ns + "expected_front_deceleration"); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, ns + "expected_rear_deceleration"); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, ns + "rear_vehicle_reaction_time"); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, ns + "rear_vehicle_safety_time_margin"); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, ns + "lateral_distance_max_threshold"); } // avoidance maneuver (lateral) @@ -174,6 +197,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = get_parameter(node, ns + "prepare_time"); p.min_prepare_distance = get_parameter(node, ns + "min_prepare_distance"); + p.remain_buffer_distance = get_parameter(node, ns + "remain_buffer_distance"); p.min_slow_down_speed = get_parameter(node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = get_parameter(node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = get_parameter(node, ns + "nominal_avoidance_speed"); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 97e047be749b5..dbad42b4ca294 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -16,8 +16,10 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -34,6 +36,7 @@ #include #include +using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; @@ -51,10 +54,12 @@ GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} +: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + parameters_{parameters}, + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); + lane_departure_checker.setVehicleInfo(vehicle_info_); occupancy_grid_map_ = std::make_shared(); @@ -117,7 +122,7 @@ GoalPlannerModule::GoalPlannerModule( void GoalPlannerModule::resetStatus() { - PUllOverStatus initial_status{}; + PullOverStatus initial_status{}; status_ = initial_status; pull_over_path_candidates_.clear(); closest_start_pose_.reset(); @@ -248,12 +253,27 @@ void GoalPlannerModule::initializeOccupancyGridMap() occupancy_grid_map_->setParam(occupancy_grid_map_param); } +void GoalPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map - if (parameters_->use_occupancy_grid) { + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { initializeOccupancyGridMap(); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + } } void GoalPlannerModule::processOnExit() @@ -271,7 +291,7 @@ bool GoalPlannerModule::isExecutionRequested() const const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; @@ -302,13 +322,19 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = - goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_) - ? calcModuleRequestLength() - : parameters_->minimum_request_length; + const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + ? calcModuleRequestLength() + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -317,15 +343,16 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } // if (A) or (B) is met execute pull over // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); if (!isCrossingPossible(current_lane, target_lane)) { @@ -337,26 +364,43 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. + if (status_.pull_over_path == nullptr) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } double GoalPlannerModule::calcModuleRequestLength() const { - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const { - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::Lanelet closest_pull_over_lanelet{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &closest_pull_over_lanelet); @@ -386,15 +430,18 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const center_pose.orientation = tf2::toMsg(tf_quat); } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) { + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) { RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); return goal_pose; } + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - distance_from_left_bound.value() + parameters_->margin_from_boundary; + sign * (distance_from_bound.value() + parameters_->margin_from_boundary); + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; @@ -404,8 +451,7 @@ ModuleStatus GoalPlannerModule::updateState() { // finish module only when the goal is fixed if ( - !goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_) && + !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler) && hasFinishedGoalPlanner()) { return ModuleStatus::SUCCESS; } @@ -442,7 +488,7 @@ bool GoalPlannerModule::planFreespacePath() mutex_.lock(); status_.pull_over_path = std::make_shared(*freespace_path); status_.current_path_idx = 0; - status_.is_safe = true; + status_.is_safe_static_objects = true; modified_goal_pose_ = goal_candidate; last_path_update_time_ = std::make_unique(clock_->now()); debug_data_.freespace_planner.is_planning = false; @@ -479,7 +525,7 @@ void GoalPlannerModule::returnToLaneParking() } mutex_.lock(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.has_decided_path = false; status_.pull_over_path = status_.lane_parking_pull_over_path; status_.current_path_idx = 0; @@ -501,9 +547,9 @@ void GoalPlannerModule::generateGoalCandidates() } // calculate goal candidates - const Pose goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); - if (goal_planner_utils::isAllowedGoalModification(route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); goal_candidates_ = goal_searcher_->search(refined_goal_pose_); } else { @@ -518,8 +564,9 @@ BehaviorModuleOutput GoalPlannerModule::plan() { generateGoalCandidates(); - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -536,7 +583,7 @@ void GoalPlannerModule::selectSafePullOverPath() const auto pull_over_path_candidates = pull_over_path_candidates_; const auto goal_candidates = goal_candidates_; mutex_.unlock(); - status_.is_safe = false; + status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -555,7 +602,7 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe = true; + status_.is_safe_static_objects = true; mutex_.lock(); status_.pull_over_path = std::make_shared(pull_over_path); status_.current_path_idx = 0; @@ -567,7 +614,7 @@ void GoalPlannerModule::selectSafePullOverPath() } // decelerate before the search area start - if (status_.is_safe) { + if (status_.is_safe_static_objects) { const auto search_start_offset_pose = calcLongitudinalOffsetPose( status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - @@ -578,7 +625,9 @@ void GoalPlannerModule::selectSafePullOverPath() decelerateBeforeSearchStart(*search_start_offset_pose, first_path); } else { // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance(parameters_->pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); for (auto & p : first_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -603,15 +652,16 @@ void GoalPlannerModule::setLanes() planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, /*forward_only_in_route*/ false); - status_.pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + status_.pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); status_.lanes = utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); } void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (status_.is_safe) { + if (status_.is_safe_static_objects) { // clear stop pose when the path is safe and activated if (isActivated()) { resetWallPoses(); @@ -640,7 +690,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe; + status_.prev_is_safe = status_.is_safe_static_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -648,7 +698,6 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - output.reference_path = getPreviousModuleOutput().reference_path; status_.prev_stop_path = output.path; // set stop path as pull over path mutex_.lock(); @@ -663,10 +712,10 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.prev_stop_path; - output.reference_path = getPreviousModuleOutput().reference_path; RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } + output.reference_path = getPreviousModuleOutput().reference_path; } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -688,7 +737,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe) { + if (status_.is_safe_static_objects) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); modified_goal.pose = modified_goal_pose_->goal_pose; @@ -736,7 +785,7 @@ bool GoalPlannerModule::hasDecidedPath() const } // if path is not safe, not decided - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return false; } @@ -835,8 +884,12 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + resetPathCandidate(); + resetPathReference(); + + path_reference_ = getPreviousModuleOutput().reference_path; + + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return planWaitingApprovalWithGoalModification(); } else { fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); @@ -853,7 +906,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe + path_candidate_ = status_.is_safe_static_objects ? std::make_shared(status_.pull_over_path->getFullPath()) : out.path; path_reference_ = getPreviousModuleOutput().reference_path; @@ -947,17 +1000,19 @@ PathWithLaneId GoalPlannerModule::generateStopPath() reference_path.points, refined_goal_pose_.position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe && !closest_start_pose_ && !search_start_offset_pose) { + if (!status_.is_safe_static_objects && !closest_start_pose_ && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = status_.is_safe ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() - : *search_start_offset_pose); + const Pose stop_pose = + status_.is_safe_static_objects + ? status_.pull_over_path->start_pose + : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -974,7 +1029,9 @@ PathWithLaneId GoalPlannerModule::generateStopPath() decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); } else { // if already passed the search start offset pose, set pull_over_velocity to reference_path. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); for (auto & p : reference_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1002,7 +1059,8 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); // calc minimum stop distance under maximum deceleration - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { return stop_path; } @@ -1044,6 +1102,10 @@ bool GoalPlannerModule::incrementPathIndex() PathWithLaneId GoalPlannerModule::getCurrentPath() const { + if (status_.pull_over_path == nullptr) { + return PathWithLaneId{}; + } + if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { return PathWithLaneId{}; } @@ -1087,7 +1149,7 @@ bool GoalPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -1101,7 +1163,7 @@ bool GoalPlannerModule::isStuck() bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto & current_path_end = getCurrentPath().points.back(); + const auto current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1159,7 +1221,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid || !occupancy_grid_map_) { + if (parameters_->use_occupancy_grid_for_path_collision_check && occupancy_grid_map_) { const bool check_out_of_range = false; if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { return true; @@ -1167,12 +1229,20 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } if (parameters_->use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; const double base_link2front = common_parameters.base_link2front; const double base_link2rear = common_parameters.base_link2rear; const double vehicle_width = common_parameters.vehicle_width; if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, + path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { return true; @@ -1202,7 +1272,8 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return false; } - const auto current_to_stop_distance = calcFeasibleDecelDistance(0.0); + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -1238,30 +1309,6 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) } } -boost::optional GoalPlannerModule::calcFeasibleDecelDistance( - const double target_velocity) const -{ - const auto v_now = planner_data_->self_odometry->twist.twist.linear.x; - const auto a_now = planner_data_->self_acceleration->accel.accel.linear.x; - const auto a_lim = parameters_->maximum_deceleration; // positive value - const auto j_lim = parameters_->maximum_jerk; - - if (v_now < target_velocity) { - return 0.0; - } - - auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( - v_now, target_velocity, a_now, -a_lim, j_lim, -1.0 * j_lim); - - if (!min_stop_distance) { - return {}; - } - - min_stop_distance = std::max(*min_stop_distance, 0.0); - - return min_stop_distance; -} - double GoalPlannerModule::calcSignedArcLengthFromEgo( const PathWithLaneId & path, const Pose & pose) const { @@ -1286,7 +1333,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const float decel_vel = std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); - const auto min_decel_distance = calcFeasibleDecelDistance(decel_vel); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -1304,7 +1352,8 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith } const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); - const auto min_stop_distance = calcFeasibleDecelDistance(0.0); + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { const auto stop_point = utils::insertStopPoint(stop_point_length, path); @@ -1318,7 +1367,9 @@ void GoalPlannerModule::decelerateBeforeSearchStart( const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. - const auto min_decel_distance = calcFeasibleDecelDistance(pull_over_velocity); + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -1426,12 +1477,83 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } +void GoalPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + goal_planner_data_.filtered_objects = filtered_objects; + goal_planner_data_.target_objects_on_lane = target_objects_on_lane; + goal_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool GoalPlannerModule::isSafePath() const +{ + const auto & pull_over_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const double forward_path_length = planner_data_->parameters.forward_path_length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto & pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, backward_path_length, forward_path_length); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel: %f, %f", terminal_velocity_and_accel.first, + terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto & ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, pull_over_lanes, current_pose.position, + objects_filtering_params_); + + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_over_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1445,9 +1567,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - - if (goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1460,7 +1580,7 @@ void GoalPlannerModule::setDebugData() } // Visualize path and related pose - if (status_.is_safe) { + if (status_.is_safe_static_objects) { add(createPoseMarkerArray( status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( @@ -1475,13 +1595,24 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + if (goal_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (goal_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } } // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); @@ -1535,10 +1666,11 @@ void GoalPlannerModule::printParkingPositionError() const bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const { const auto & route_handler = planner_data_->route_handler; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 9e94a234a7f48..a3b0c6adf9721 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -32,18 +32,17 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { GoalPlannerParameters p; + const std::string base_ns = "goal_planner."; // general params { - std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); } // goal search { - std::string ns = "goal_planner.goal_search."; + const std::string ns = base_ns + "goal_search."; p.search_priority = node->declare_parameter(ns + "search_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); @@ -73,9 +72,12 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { std::string ns = "goal_planner.occupancy_grid."; - p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); - p.use_occupancy_grid_for_longitudinal_margin = - node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); + p.use_occupancy_grid_for_goal_search = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_search"); + p.use_occupancy_grid_for_path_collision_check = + node->declare_parameter(ns + "use_occupancy_grid_for_path_collision_check"); + p.use_occupancy_grid_for_goal_longitudinal_margin = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_longitudinal_margin"); p.occupancy_grid_collision_check_margin = node->declare_parameter(ns + "occupancy_grid_collision_check_margin"); p.theta_size = node->declare_parameter(ns + "theta_size"); @@ -84,18 +86,21 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // object recognition { - std::string ns = "goal_planner.object_recognition."; + const std::string ns = base_ns + "object_recognition."; p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); p.object_recognition_collision_check_max_extra_stopping_margin = node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); } // pull over general params { - std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; + p.pull_over_minimum_request_length = + node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity"); @@ -106,7 +111,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // shift parking { - std::string ns = "goal_planner.pull_over.shift_parking."; + const std::string ns = base_ns + "pull_over.shift_parking."; p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); @@ -118,7 +123,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking forward { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); @@ -134,7 +139,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking backward { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = @@ -151,7 +156,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking general params { - std::string ns = "goal_planner.pull_over.freespace_parking."; + const std::string ns = base_ns + "pull_over.freespace_parking."; p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); p.freespace_parking_algorithm = node->declare_parameter(ns + "freespace_parking_algorithm"); @@ -174,7 +179,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking search config { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; p.freespace_parking_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_parking_common_parameters.angle_goal_range = @@ -191,14 +196,14 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking costmap configs { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; p.freespace_parking_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace parking astar { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -208,7 +213,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking rrtstar { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -218,9 +223,127 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // debug { - std::string ns = "goal_planner.debug."; + const std::string ns = base_ns + "debug."; p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); } @@ -241,8 +364,6 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } parameters_ = std::make_shared(p); - - left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; } void GoalPlannerModuleManager::updateModuleParams( @@ -259,12 +380,26 @@ void GoalPlannerModuleManager::updateModuleParams( }); } +bool GoalPlannerModuleManager::isAlwaysExecutableModule() const +{ + // enable AlwaysExecutable whenever goal modification is not allowed + // because only minor path refinements are made for fixed goals + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return true; + } + + return false; +} + bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -273,10 +408,13 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { + if (isAlwaysExecutableModule()) { + return true; + } + // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification( - planner_data_->route_handler, left_side_parking_)) { + if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index e33b1d9b13873..a31eeed88a275 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -297,11 +297,10 @@ void LaneChangeInterface::setObjectDebugVisualization() const if (!parameters_->publish_debug_marker) { return; } + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; - using marker_utils::lane_change_markers::showLerpedPose; - using marker_utils::lane_change_markers::showObjectInfo; - using marker_utils::lane_change_markers::showPolygon; - using marker_utils::lane_change_markers::showPolygonPose; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); @@ -314,14 +313,14 @@ void LaneChangeInterface::setObjectDebugVisualization() const add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); if (!debug_data.empty()) { - add(showObjectInfo(debug_data, "object_debug_info")); - add(showLerpedPose(debug_data, "ego_predicted_path")); + add(showSafetyCheckInfo(debug_data, "object_debug_info")); + add(showPredictedPath(debug_data, "ego_predicted_path")); add(showPolygon(debug_data, "ego_and_target_polygon_relation")); } if (!debug_after_approval.empty()) { - add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval")); - add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval")); + add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval")); + add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval")); add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval")); } } @@ -334,11 +333,11 @@ std::shared_ptr LaneChangeInterface::get_debug_msg_arra for (const auto & [uuid, debug_data] : debug_data) { LaneChangeDebugMsg debug_msg; debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.allow_lane_change; + debug_msg.allow_lane_change = debug_data.is_safe; debug_msg.is_front = debug_data.is_front; - debug_msg.relative_distance = debug_data.relative_to_ego; - debug_msg.failed_reason = debug_data.failed_reason; - debug_msg.velocity = debug_data.object_twist.linear.x; + debug_msg.failed_reason = debug_data.unsafe_reason; + debug_msg.velocity = + std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y); debug_msg_array.lane_change_info.push_back(debug_msg); } lane_change_debug_msg_array_ = debug_msg_array; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 18c669a207095..7692cd624bffd 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -85,6 +85,36 @@ LaneChangeModuleManager::LaneChangeModuleManager( get_parameter(node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = get_parameter(node, parameter("use_all_predicted_path")); + p.rss_params.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + + p.rss_params_for_abort.longitudinal_distance_min_threshold = + get_parameter(node, parameter("safety_check.longitudinal_distance_min_threshold")); + p.rss_params_for_abort.longitudinal_velocity_delta_time = + get_parameter(node, parameter("safety_check.longitudinal_velocity_delta_time")); + p.rss_params_for_abort.front_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_front_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_deceleration = + get_parameter(node, parameter("safety_check.expected_rear_deceleration_for_abort")); + p.rss_params_for_abort.rear_vehicle_reaction_time = + get_parameter(node, parameter("safety_check.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = + get_parameter(node, parameter("safety_check.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = + get_parameter(node, parameter("safety_check.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; @@ -264,8 +294,8 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // safety check { std::string ns = "avoidance.safety_check."; - p.safety_check_hysteresis_factor = - get_parameter(node, ns + "safety_check_hysteresis_factor"); + p.hysteresis_factor_expand_rate = + get_parameter(node, ns + "hysteresis_factor_expand_rate"); } avoidance_parameters_ = std::make_shared(p); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index f4ef15ad50cd6..f05b5572f6ce6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/normal.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" @@ -608,9 +609,7 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = utils::lane_change::filterObject( - objects, current_lanes, target_lanes, target_backward_lanes, current_pose, route_handler, - *lane_change_parameters_); + const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -641,6 +640,113 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( return target_objects; } +LaneChangeTargetObjectIndices NormalLaneChange::filterObject( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const lanelet::ConstLanelets & target_backward_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + const auto & objects = *planner_data_->dynamic_object; + + // Guard + if (objects.objects.empty()) { + return {}; + } + + // Get path + const auto path = + route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_path = + route_handler.getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + + const auto current_polygon = + utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto target_backward_polygon = utils::lane_change::createPolygon( + target_backward_lanes, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); + + LaneChangeTargetObjectIndices filtered_obj_indices; + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; + const auto extended_object = + utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + + // ignore specific object types + if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) { + continue; + } + + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); + + // calc distance from the current ego position + double max_dist_ego_to_obj = std::numeric_limits::lowest(); + double min_dist_ego_to_obj = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + const double dist_ego_to_obj = + motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); + min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); + } + + const auto is_lateral_far = [&]() { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet( + current_lanes, object.kinematics.initial_pose_with_covariance.pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (common_parameters.vehicle_width / 2); + }; + + // ignore static object that are behind the ego vehicle + if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { + continue; + } + + // check if the object intersects with target lanes + if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { + // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target + // lanes + + if (max_dist_ego_to_obj >= 0 || is_lateral_far()) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } + } + + // check if the object intersects with target backward lanes + if ( + target_backward_polygon && + boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } + + // check if the object intersects with current lanes + if ( + current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && + max_dist_ego_to_obj >= 0.0) { + // check only the objects that are in front of the ego vehicle + filtered_obj_indices.current_lane.push_back(i); + continue; + } + + // ignore all objects that are behind the ego vehicle and not on the current and target + // lanes + if (max_dist_ego_to_obj < 0.0) { + continue; + } + + filtered_obj_indices.other_lane.push_back(i); + } + + return filtered_obj_indices; +} + PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -937,8 +1043,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, common_parameters.expected_front_deceleration, - common_parameters.expected_rear_deceleration, object_debug_); + *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); if (is_safe) { return true; @@ -951,7 +1056,6 @@ bool NormalLaneChange::getLaneChangePaths( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { - const auto & common_parameters = getCommonParam(); const auto & path = status_.lane_change_path; const auto & current_lanes = status_.current_lanes; const auto & target_lanes = status_.target_lanes; @@ -960,8 +1064,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; const auto safety_status = isLaneChangePathSafe( - path, target_objects, common_parameters.expected_front_deceleration_for_abort, - common_parameters.expected_rear_deceleration_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1218,8 +1321,8 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const double front_decel, const double rear_decel, - std::unordered_map & debug_data) const + const utils::path_safety_checker::RSSparams & rss_params, + CollisionCheckDebugMap & debug_data) const { PathSafetyStatus path_safety_status; @@ -1254,36 +1357,17 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } - const auto assignDebugData = [](const ExtendedPredictedObject & obj) { - CollisionCheckDebug debug; - debug.current_pose = obj.initial_pose.pose; - debug.current_twist = obj.initial_twist.twist; - return std::make_pair(tier4_autoware_utils::toHexString(obj.uuid), debug); - }; - - const auto updateDebugInfo = - [&debug_data](std::pair & obj, bool is_allowed) { - const auto & key = obj.first; - auto & element = obj.second; - element.allow_lane_change = is_allowed; - if (debug_data.find(key) != debug_data.end()) { - debug_data[key] = element; - } else { - debug_data.insert(obj); - } - }; - for (const auto & obj : collision_check_objects) { - auto current_debug_data = assignDebugData(obj); - current_debug_data.second.ego_predicted_path.push_back(debug_predicted_path); + auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); for (const auto & obj_path : obj_predicted_paths) { if (!utils::path_safety_checker::checkCollision( - path, ego_predicted_path, obj, obj_path, common_parameters, front_decel, rear_decel, + path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second)) { path_safety_status.is_safe = false; - updateDebugInfo(current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= @@ -1291,7 +1375,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( path, current_pose, common_parameters.vehicle_info, obj_polygon); } } - updateDebugInfo(current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap( + debug_data, current_debug_data, path_safety_status.is_safe); } return path_safety_status; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index ec18d55a31069..f5cd60e315e68 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -36,6 +36,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); p.th_turn_signal_on_lateral_offset = node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); + p.th_distance_to_middle_of_the_road = + node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); @@ -148,6 +150,124 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = base_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 98796d52c6e73..89e9200fee250 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -97,6 +99,14 @@ BehaviorModuleOutput StartPlannerModule::run() return plan(); } +void StartPlannerModule::processOnEntry() +{ + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + } +} + void StartPlannerModule::processOnExit() { resetPathCandidate(); @@ -106,9 +116,16 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { - // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const double lateral_distance_to_center_lane = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + if (std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road) { + return false; + } + + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > parameters_->th_arrived_distance) { @@ -116,7 +133,7 @@ bool StartPlannerModule::isExecutionRequested() const } // Check if ego arrives at goal - const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { @@ -133,41 +150,21 @@ bool StartPlannerModule::isExecutionRequested() const return false; } - // Create vehicle footprint - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); - - // Check if ego is not out of lanes - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - auto lanes = current_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } - - if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { - return false; - } - return true; } bool StartPlannerModule::isExecutionReady() const { + if (status_.pull_out_path.partial_paths.empty()) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -197,6 +194,7 @@ BehaviorModuleOutput StartPlannerModule::plan() getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -209,11 +207,12 @@ BehaviorModuleOutput StartPlannerModule::plan() } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -279,6 +278,15 @@ CandidateOutput StartPlannerModule::planCandidate() const return CandidateOutput{}; } +void StartPlannerModule::initializeSafetyCheckParameters() +{ + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); +} + PathWithLaneId StartPlannerModule::getFullPath() const { // combine partial pull out path @@ -310,16 +318,18 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } BehaviorModuleOutput output; - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); clearWaitingApproval(); const auto output = generateStopOutput(); setDebugData(); // use status updated in generateStopOutput() + updateRTCStatus(0, 0); return output; } @@ -406,8 +416,8 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const } void StartPlannerModule::planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority) + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority) { // check if start pose candidates are valid if (start_pose_candidates.empty()) { @@ -418,6 +428,10 @@ void StartPlannerModule::planWithPriority( // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); + // Set back_finished to true if the current start pose is same to refined_start_pose + status_.back_finished = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); // not found safe path @@ -427,7 +441,7 @@ void StartPlannerModule::planWithPriority( // use current path if back is not needed if (status_.back_finished) { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); @@ -448,7 +462,7 @@ void StartPlannerModule::planWithPriority( // Update status variables with the next path information { const std::lock_guard lock(mutex_); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.pull_out_path = *pull_out_path_next; status_.pull_out_start_pose = pull_out_start_pose_next; status_.planner_type = planner->getPlannerType(); @@ -493,9 +507,6 @@ void StartPlannerModule::planWithPriority( for (const auto & p : order_priority) { if (is_safe_with_pose_planner(p.first, p.second)) { - const std::lock_guard lock(mutex_); - // Set back_finished flag based on the current index - status_.back_finished = p.first == 0; return; } } @@ -503,7 +514,7 @@ void StartPlannerModule::planWithPriority( // not found safe path if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); - status_.is_safe = false; + status_.is_safe_static_objects = false; status_.planner_type = PlannerType::NONE; } } @@ -564,10 +575,15 @@ std::vector StartPlannerModule::generateDrivableLanes( const PathWithLaneId & path) const { const auto path_road_lanes = getPathRoadLanes(path); - if (!path_road_lanes.empty()) { - return utils::generateDrivableLanesWithShoulderLanes( - getPathRoadLanes(path), status_.pull_out_lanes); + lanelet::ConstLanelets shoulder_lanes; + const auto & rh = planner_data_->route_handler; + std::copy_if( + status_.pull_out_lanes.begin(), status_.pull_out_lanes.end(), + std::back_inserter(shoulder_lanes), + [&rh](const auto & pull_out_lane) { return rh->isShoulderLanelet(pull_out_lane); }); + + return utils::generateDrivableLanesWithShoulderLanes(path_road_lanes, shoulder_lanes); } // if path_road_lanes is empty, use only pull_out_lanes as drivable lanes @@ -598,7 +614,7 @@ void StartPlannerModule::updatePullOutStatus() // skip updating if enough time has not passed for preventing chattering between back and // start_planner - if (!has_received_new_route && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!has_received_new_route) { if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -613,9 +629,24 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + // refine start pose with pull out lanes. + // 1) backward driving is not allowed: use refined pose just as start pose. + // 2) backward driving is allowed: use refined pose to check if backward driving is needed. + const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const auto refined_start_pose = calcLongitudinalOffsetPose( + start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); + if (!refined_start_pose) return; + // search pull out start candidates backward - std::vector start_pose_candidates = searchPullOutStartPoses(); - planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); + const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { + if (parameters_->enable_back) { + return searchPullOutStartPoses(start_pose_candidates_path); + } + return {*refined_start_pose}; + }); + + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); checkBackFinished(); if (!status_.back_finished) { @@ -625,20 +656,34 @@ void StartPlannerModule::updatePullOutStatus() } } -std::vector StartPlannerModule::searchPullOutStartPoses() +PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { - std::vector pull_out_start_pose{}; - const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + auto path = planner_data_->route_handler->getCenterLinePath( status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + } + + return path; +} + +std::vector StartPlannerModule::searchPullOutStartPoses( + const PathWithLaneId & start_pose_candidates) const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + std::vector pull_out_start_pose{}; + // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -646,28 +691,19 @@ std::vector StartPlannerModule::searchPullOutStartPoses() const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : backward_shoulder_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); - } - - // if backward driving is disable, just refine current pose to the lanes - if (!parameters_->enable_back) { - const auto refined_pose = - calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); - if (refined_pose) { - pull_out_start_pose.push_back(*refined_pose); - } - return pull_out_start_pose; - } + // Set the maximum backward distance less than the distance from the vehicle's base_link to the + // lane's rearmost point to prevent lane departure. + const double s_current = + lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose).length; + const double max_back_distance = std::clamp( + s_current - planner_data_->parameters.base_link2rear, 0.0, parameters_->max_back_distance); // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - for (double back_distance = 0.0; back_distance <= parameters_->max_back_distance; + for (double back_distance = 0.0; back_distance <= max_back_distance; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( - backward_shoulder_path.points, current_pose.position, -back_distance); + start_pose_candidates.points, current_pose.position, -back_distance); if (!backed_pose) { continue; } @@ -675,10 +711,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - double length_to_lane_end = 0.0; - for (const auto & lane : status_.pull_out_lanes) { - length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); - } + + const double length_to_lane_end = std::accumulate( + std::begin(status_.pull_out_lanes), std::end(status_.pull_out_lanes), 0.0, + [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); }); const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( @@ -713,7 +749,7 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished || !status_.is_safe) { + if (!status_.back_finished || !status_.is_safe_static_objects) { return false; } @@ -800,7 +836,7 @@ bool StartPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe) { + if (!status_.is_safe_static_objects) { return true; } @@ -921,6 +957,70 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +void StartPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + start_planner_data_.filtered_objects = filtered_objects; + start_planner_data_.target_objects_on_lane = target_objects_on_lane; + start_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool StartPlannerModule::isSafePath() const +{ + // TODO(Sugahara): should safety check for backward path + + const auto & pull_out_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const auto & current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto & ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto & filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + + const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + current_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_out_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -1011,7 +1111,7 @@ bool StartPlannerModule::planFreespacePath() status_.pull_out_path = *freespace_path; status_.pull_out_start_pose = current_pose; status_.planner_type = freespace_planner_->getPlannerType(); - status_.is_safe = true; + status_.is_safe_static_objects = true; status_.back_finished = true; return true; } @@ -1038,8 +1138,10 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1057,13 +1159,24 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 2e6d8f982b8a2..10766b7aec416 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -645,7 +645,7 @@ void fillAvoidanceNecessity( } // TRUE -> ? (check with hysteresis factor) - object_data.avoid_required = check_necessity(parameters->safety_check_hysteresis_factor); + object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } void fillObjectStoppableJudge( @@ -1310,7 +1310,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) + const bool is_object_front, const std::shared_ptr & parameters) { if (path.points.empty()) { return {}; @@ -1319,7 +1319,8 @@ std::vector convertToPredictedPath( const auto & acceleration = parameters->max_acceleration; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = is_object_front ? parameters->time_horizon_for_front_object + : parameters->time_horizon_for_rear_object; const auto & time_resolution = parameters->safety_check_time_resolution; const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); @@ -1350,7 +1351,8 @@ ExtendedPredictedObject transform( extended_object.shape = object.shape; const auto & obj_velocity = extended_object.initial_twist.twist.linear.x; - const auto & time_horizon = parameters->safety_check_time_horizon; + const auto & time_horizon = + std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object); const auto & time_resolution = parameters->safety_check_time_resolution; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index a5ccfc9517771..840597e1ec95d 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -163,6 +163,7 @@ void GeometricParallelParking::clearPaths() current_path_idx_ = 0; arc_paths_.clear(); paths_.clear(); + pairs_terminal_velocity_and_accel_.clear(); } bool GeometricParallelParking::planPullOver( @@ -182,6 +183,7 @@ bool GeometricParallelParking::planPullOver( if (is_forward) { // When turning forward to the right, the front left goes out, // so reduce the steer angle at that time for seach no lane departure path. + // TODO(Sugahara): define in the config constexpr double start_pose_offset = 0.0; constexpr double min_steer_rad = 0.05; constexpr double steer_interval = 0.1; @@ -280,6 +282,10 @@ bool GeometricParallelParking::planPullOut( PathWithLaneId road_center_line_path = planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + if (road_center_line_path.points.empty()) { + continue; + } + // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; @@ -445,7 +451,6 @@ std::vector GeometricParallelParking::planOneTrial( if (std::abs(end_pose_offset) > 0) { PathPointWithLaneId straight_point{}; straight_point.point.pose = goal_pose; - // setLaneIds(straight_point); path_turn_right.points.push_back(straight_point); } @@ -481,6 +486,19 @@ std::vector GeometricParallelParking::planOneTrial( paths_.push_back(path_turn_left); paths_.push_back(path_turn_right); + // set terminal velocity and acceleration(temporary implementation) + if (is_forward) { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.forward_parking_velocity, 0.0)); + } else { + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + pairs_terminal_velocity_and_accel_.push_back( + std::make_pair(parameters_.backward_parking_velocity, 0.0)); + } + // set pull_over start and end pose // todo: make start and end pose for pull_out start_pose_ = start_pose; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index f4aabb13b8916..9322350877ad1 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include #include @@ -55,16 +56,16 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) const Pose end_pose = use_back_ ? goal_pose : tier4_autoware_utils::calcOffsetPose(goal_pose, -straight_distance, 0.0, 0.0); - const bool found_path = planner_->makePlan(current_pose, end_pose); - if (!found_path) { + if (!planner_->makePlan(current_pose, end_pose)) { return {}; } const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } @@ -113,18 +114,21 @@ boost::optional FreespacePullOver::plan(const Pose & goal_pose) addPose(goal_pose); } - utils::correctDividedPathVelocity(partial_paths); + std::vector> pairs_terminal_velocity_and_accel{}; + pairs_terminal_velocity_and_accel.resize(partial_paths.size()); + utils::start_goal_planner_common::modifyVelocityByDirection( + partial_paths, pairs_terminal_velocity_and_accel, velocity_, 0); + // Check if driving forward for each path, return empty if not for (auto & path : partial_paths) { - const auto is_driving_forward = motion_utils::isDrivingForward(path.points); - if (!is_driving_forward) { - // path points is less than 2 + if (!motion_utils::isDrivingForward(path.points)) { return {}; } } PullOverPath pull_over_path{}; pull_over_path.partial_paths = partial_paths; + pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; pull_over_path.start_pose = current_pose; pull_over_path.end_pose = goal_pose; pull_over_path.type = getPlannerType(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 6a5ccc827db29..ed0106d891bb4 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -47,13 +47,13 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - auto lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -62,7 +62,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_); if (!found_valid_path) { return {}; } @@ -75,6 +75,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths = planner_.getPaths(); + pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); pull_over_path.start_pose = planner_.getStartPose(); pull_over_path.end_pose = planner_.getArcEndPose(); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e57dcc319dc5..2f67382eab8de 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -53,9 +53,13 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); auto lanes = utils::getExtendedCurrentLanes( planner_data_, backward_length, forward_length, /*forward_only_in_route*/ false); @@ -69,14 +73,13 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), parameters_.goal_search_interval); - const auto [shoulder_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { - const Pose & center_pose = p.point.pose; + // todo(kosuke55): fix orientation for inverseTransformPoint temporarily + Pose center_pose = p.point.pose; + center_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -85,13 +88,14 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) continue; } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) continue; + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) continue; - const double sign = left_side_parking_ ? 1.0 : -1.0; + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - sign * (std::abs(distance_from_left_bound.value()) - margin_from_boundary); + -distance_from_bound.value() + sign * margin_from_boundary; const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( @@ -102,7 +106,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_search_pose, 0, -sign * dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); @@ -149,15 +153,18 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const continue; } - // check margin with pull over lane objects - const auto pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); - const auto [shoulder_lane_objects, others] = + // check longitudinal margin with pull over lane objects + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( - goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { goal_candidate.is_safe = false; @@ -170,7 +177,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const bool GoalSearcher::checkCollision(const Pose & pose) const { - if (parameters_.use_occupancy_grid) { + if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); const auto idx = pose2index( occupancy_grid_map_->getMap(), pose_grid_coords, occupancy_grid_map_->getParam().theta_size); @@ -181,8 +188,16 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } if (parameters_.use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, *(planner_data_->dynamic_object), + vehicle_footprint_, pose, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin)) { return true; } @@ -193,7 +208,9 @@ bool GoalSearcher::checkCollision(const Pose & pose) const bool GoalSearcher::checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const { - if (parameters_.use_occupancy_grid && parameters_.use_occupancy_grid_for_longitudinal_margin) { + if ( + parameters_.use_occupancy_grid_for_goal_search && + parameters_.use_occupancy_grid_for_goal_longitudinal_margin) { constexpr bool check_out_of_range = false; const double offset = std::max( parameters_.longitudinal_margin - parameters_.occupancy_grid_collision_check_margin, 0.0); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index 2f5c1d9b05b7f..75b2a8237687d 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -49,16 +50,16 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = - goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = - generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); + generatePullOverPath(road_lanes, pull_over_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -170,6 +171,8 @@ boost::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } + shifted_path.path = utils::removeInverseOrderPathPoints(shifted_path.path); + // set the same z as the goal for (auto & p : shifted_path.path.points) { p.point.pose.position.z = goal_pose.position.z; @@ -204,6 +207,7 @@ boost::optional ShiftPullOver::generatePullOverPath( PullOverPath pull_over_path{}; pull_over_path.type = getPlannerType(); pull_over_path.partial_paths.push_back(shifted_path.path); + pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 19b6bd27ea07c..aa3380c2b99df 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -55,36 +55,37 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith return path; } -lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) +lanelet::ConstLanelets getPullOverLanes( + const RouteHandler & route_handler, const bool left_side, const double backward_distance, + const double forward_distance) { - const Pose goal_pose = route_handler.getGoalPose(); + const Pose goal_pose = route_handler.getOriginalGoalPose(); + + // Buffer to get enough lanes in front of the goal, need much longer than the pull over distance. + // In the case of loop lanes, it may not be possible to extend the lane forward. + // todo(kosuek55): automatically calculates this distance. + const double backward_distance_with_buffer = backward_distance + 100; lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( route_handler.getShoulderLanelets(), goal_pose, &target_shoulder_lane)) { // pull over on shoulder lane - return route_handler.getShoulderLaneletSequence(target_shoulder_lane, goal_pose); + return route_handler.getShoulderLaneletSequence( + target_shoulder_lane, goal_pose, backward_distance_with_buffer, forward_distance); } - // pull over on road lane lanelet::ConstLanelet closest_lane{}; route_handler.getClosestLaneletWithinRoute(goal_pose, &closest_lane); - lanelet::ConstLanelet outermost_lane{}; if (left_side) { - outermost_lane = route_handler.getMostLeftLanelet(closest_lane); + outermost_lane = route_handler.getMostLeftLanelet(closest_lane, false, true); } else { - outermost_lane = route_handler.getMostRightLanelet(closest_lane); + outermost_lane = route_handler.getMostRightLanelet(closest_lane, false, true); } - lanelet::ConstLanelet outermost_shoulder_lane; - if (route_handler.getLeftShoulderLanelet(outermost_lane, &outermost_shoulder_lane)) { - return route_handler.getShoulderLaneletSequence(outermost_shoulder_lane, goal_pose); - } - - const bool dist = std::numeric_limits::max(); constexpr bool only_route_lanes = false; - return route_handler.getLaneletSequence(outermost_lane, dist, dist, only_route_lanes); + return route_handler.getLaneletSequence( + outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes); } PredictedObjects filterObjectsByLateralDistance( @@ -181,25 +182,22 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool isAllowedGoalModification(const std::shared_ptr & route_handler) { - return route_handler->isAllowedGoalModification() || - checkOriginalGoalIsInShoulder(route_handler, left_side_parking); + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); } -bool checkOriginalGoalIsInShoulder( - const std::shared_ptr & route_handler, const bool left_side_parking) +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) { const Pose & goal_pose = route_handler->getGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); - const lanelet::ConstLanelets pull_over_lanes = - goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking); - lanelet::ConstLanelet target_lane{}; - lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); + lanelet::ConstLanelet closest_shoulder_lane{}; + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } - return route_handler->isShoulderLanelet(target_lane) && - lanelet::utils::isInLanelet(goal_pose, target_lane, 0.1); + return false; } } // namespace goal_planner_utils diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 34f5f2065a0f0..a277e72c8660d 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1063,88 +1063,6 @@ std::optional createPolygon( return lanelet::utils::to2D(polygon_3d).basicPolygon(); } -LaneChangeTargetObjectIndices filterObject( - const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes, - const Pose & current_pose, const RouteHandler & route_handler, - const LaneChangeParameters & lane_change_parameter) -{ - // Guard - if (objects.objects.empty()) { - return {}; - } - - // Get path - const auto path = - route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); - - const auto current_polygon = - createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = - createPolygon(target_backward_lanes, 0.0, std::numeric_limits::max()); - - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & object = objects.objects.at(i); - const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x; - - // ignore specific object types - if (!isTargetObjectType(object, lane_change_parameter)) { - continue; - } - - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - - // calc distance from the current ego position - double max_dist_ego_to_obj = std::numeric_limits::lowest(); - for (const auto & polygon_p : obj_polygon.outer()) { - const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); - max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); - } - - // ignore static object that are behind the ego vehicle - if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) { - continue; - } - - // check if the object intersects with target lanes - if (target_polygon && boost::geometry::intersects(target_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with target backward lanes - if ( - target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { - filtered_obj_indices.target_lane.push_back(i); - continue; - } - - // check if the object intersects with current lanes - if ( - current_polygon && boost::geometry::intersects(current_polygon.value(), obj_polygon) && - max_dist_ego_to_obj >= 0.0) { - // check only the objects that are in front of the ego vehicle - filtered_obj_indices.current_lane.push_back(i); - continue; - } - - // ignore all objects that are behind the ego vehicle and not on the current and target - // lanes - if (max_dist_ego_to_obj < 0.0) { - continue; - } - - filtered_obj_indices.other_lane.push_back(i); - } - - return filtered_obj_indices; -} - ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters) diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 33afe0fe6642f..23016b01cb9ab 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -37,7 +37,7 @@ PredictedObjects filterObjects( PredictedObjects filtered_objects; - filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold); + filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); filterObjectsByClass(filtered_objects, target_object_types); @@ -51,13 +51,19 @@ PredictedObjects filterObjects( return filtered_objects; } -PredictedObjects filterObjectsByVelocity(const PredictedObjects & objects, double lim_v) +PredictedObjects filterObjectsByVelocity( + const PredictedObjects & objects, const double velocity_threshold, + const bool remove_above_threshold) { - return filterObjectsByVelocity(objects, -lim_v, lim_v); + if (remove_above_threshold) { + return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); + } else { + return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); + } } PredictedObjects filterObjectsByVelocity( - const PredictedObjects & objects, double min_v, double max_v) + const PredictedObjects & objects, double velocity_threshold, double max_velocity) { PredictedObjects filtered; filtered.header = objects.header; @@ -65,7 +71,7 @@ PredictedObjects filterObjectsByVelocity( const auto v_norm = std::hypot( obj.kinematics.initial_twist_with_covariance.twist.linear.x, obj.kinematics.initial_twist_with_covariance.twist.linear.y); - if (min_v < v_norm && v_norm < max_v) { + if (velocity_threshold < v_norm && v_norm < max_velocity) { filtered.objects.push_back(obj); } } @@ -337,13 +343,13 @@ TargetObjectsOnLane createTargetObjectsOnLane( }; // TODO(Sugahara): Consider shoulder and other lane objects - if (object_lane_configuration.check_current_lane) { + if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } - if (object_lane_configuration.check_left_lane) { + if (object_lane_configuration.check_left_lane && !all_left_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets); } - if (object_lane_configuration.check_right_lane) { + if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 34ba607938385..1838dc6bdc07b 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -30,6 +30,25 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +bool isTargetObjectFront( + const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const auto ego_offset_pose = + tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); + + // check all edges in the polygon + for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); + if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { + return true; + } + } + + return false; +} + bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) @@ -62,8 +81,8 @@ Polygon2d createExtendedPolygon( const double lat_offset = width / 2.0 + lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = lat_offset; + debug.extended_polygon_lon_offset = lon_offset; + debug.extended_polygon_lat_offset = lat_offset; } const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); @@ -113,8 +132,8 @@ Polygon2d createExtendedPolygon( const double right_lat_offset = min_y - lat_margin; { - debug.longitudinal_offset = lon_offset; - debug.lateral_offset = (left_lat_offset + right_lat_offset) / 2; + debug.extended_polygon_lon_offset = lon_offset; + debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2; } const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); @@ -133,10 +152,22 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +PredictedPath convertToPredictedPath( + const std::vector & path, const double time_resolution) +{ + PredictedPath predicted_path; + predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); + predicted_path.path.resize(path.size()); + + for (size_t i = 0; i < path.size(); ++i) { + predicted_path.path.at(i) = path.at(i).pose; + } + return predicted_path; +} + double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, - const double front_object_deceleration, const double rear_object_deceleration, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { const auto stoppingDistance = [](const auto vehicle_velocity, const auto vehicle_accel) { // compensate if user accidentally set the deceleration to some positive value @@ -145,23 +176,23 @@ double calcRssDistance( }; const double & reaction_time = - params.rear_vehicle_reaction_time + params.rear_vehicle_safety_time_margin; + rss_params.rear_vehicle_reaction_time + rss_params.rear_vehicle_safety_time_margin; const double front_object_stop_length = - stoppingDistance(front_object_velocity, front_object_deceleration); + stoppingDistance(front_object_velocity, rss_params.front_vehicle_deceleration); const double rear_object_stop_length = rear_object_velocity * reaction_time + - stoppingDistance(rear_object_velocity, rear_object_deceleration); + stoppingDistance(rear_object_velocity, rss_params.rear_vehicle_deceleration); return rear_object_stop_length - front_object_stop_length; } double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params) + const RSSparams & rss_params) { - const double & lon_threshold = params.longitudinal_distance_min_threshold; + const double & lon_threshold = rss_params.longitudinal_distance_min_threshold; const auto max_vel = std::max(front_object_velocity, rear_object_velocity); - return params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; + return rss_params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } boost::optional calcInterpolatedPoseWithVelocity( @@ -215,14 +246,18 @@ boost::optional getInterpolatedPoseWithVeloci } bool checkCollision( - const PathWithLaneId & planned_path, + [[maybe_unused]] const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, const ExtendedPredictedObject & target_object, const PredictedPathWithPolygon & target_object_path, - const BehaviorPathPlannerParameters & common_parameters, const double front_object_deceleration, - const double rear_object_deceleration, CollisionCheckDebug & debug) + const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, + double hysteresis_factor, CollisionCheckDebug & debug) { - debug.lerped_path.reserve(target_object_path.path.size()); + { + debug.ego_predicted_path = predicted_ego_path; + debug.obj_predicted_path = target_object_path.path; + debug.current_obj_pose = target_object.initial_pose.pose; + } for (const auto & obj_pose_with_poly : target_object_path.path) { const auto & current_time = obj_pose_with_poly.time; @@ -246,37 +281,34 @@ bool checkCollision( const auto & ego_velocity = interpolated_data->velocity; { - debug.lerped_path.push_back(ego_pose); debug.expected_ego_pose = ego_pose; debug.expected_obj_pose = obj_pose; - debug.ego_polygon = ego_polygon; - debug.obj_polygon = obj_polygon; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; } // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { - debug.failed_reason = "overlap_polygon"; + debug.unsafe_reason = "overlap_polygon"; return false; } // compute which one is at the front of the other - const bool is_object_front = - isTargetObjectFront(planned_path, ego_pose, ego_vehicle_info, obj_polygon); + const bool is_object_front = isTargetObjectFront(ego_pose, obj_polygon, ego_vehicle_info); const auto & [front_object_velocity, rear_object_velocity] = is_object_front ? std::make_pair(object_velocity, ego_velocity) : std::make_pair(ego_velocity, object_velocity); // compute rss dist - const auto rss_dist = calcRssDistance( - front_object_velocity, rear_object_velocity, front_object_deceleration, - rear_object_deceleration, common_parameters); + const auto rss_dist = + calcRssDistance(front_object_velocity, rear_object_velocity, rss_parameters); // minimum longitudinal length const auto min_lon_length = - calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, common_parameters); + calcMinimumLongitudinalLength(front_object_velocity, rear_object_velocity, rss_parameters); - const auto & lon_offset = std::max(rss_dist, min_lon_length); - const auto & lat_margin = common_parameters.lateral_distance_max_threshold; + const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; + const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) @@ -288,15 +320,15 @@ bool checkCollision( { debug.rss_longitudinal = rss_dist; - debug.ego_to_obj_margin = min_lon_length; - debug.ego_polygon = extended_ego_polygon; - debug.obj_polygon = extended_obj_polygon; + debug.inter_vehicle_distance = min_lon_length; + debug.extended_ego_polygon = extended_ego_polygon; + debug.extended_obj_polygon = extended_obj_polygon; debug.is_front = is_object_front; } // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { - debug.failed_reason = "overlap_extended_polygon"; + debug.unsafe_reason = "overlap_extended_polygon"; return false; } } diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index ec14a064bf51b..c4bfb62276d42 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -418,6 +418,7 @@ void correctDividedPathVelocity(std::vector & divided_paths) { for (auto & path : divided_paths) { const auto is_driving_forward = motion_utils::isDrivingForward(path.points); + // If the number of points in the path is less than 2, don't correct the velocity if (!is_driving_forward) { continue; } diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp new file mode 100644 index 0000000000000..cff6e9f81f02e --- /dev/null +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -0,0 +1,155 @@ +// 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. + +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" + +namespace behavior_path_planner::utils::start_goal_planner_common +{ + +using motion_utils::calcDecelDistWithJerkAndAccConstraints; + +boost::optional calcFeasibleDecelDistance( + std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, + const double target_velocity) +{ + const auto v_now = planner_data->self_odometry->twist.twist.linear.x; + const auto a_now = planner_data->self_acceleration->accel.accel.linear.x; + + if (v_now < target_velocity) { + return 0.0; + } + + auto min_stop_distance = calcDecelDistWithJerkAndAccConstraints( + v_now, target_velocity, a_now, -acc_lim, jerk_lim, -1.0 * jerk_lim); + + if (!min_stop_distance) { + return {}; + } + + min_stop_distance = std::max(*min_stop_distance, 0.0); + + return min_stop_distance; +} + +void modifyVelocityByDirection( + std::vector & paths, + std::vector> & terminal_vel_acc_pairs, const double target_velocity, + const double acceleration) +{ + assert(paths.size() == terminal_vel_acc_pairs.size()); + + auto path_itr = std::begin(paths); + auto pair_itr = std::begin(terminal_vel_acc_pairs); + + for (; path_itr != std::end(paths); ++path_itr, ++pair_itr) { + const auto is_driving_forward = motion_utils::isDrivingForward(path_itr->points); + + // If the number of points in the path is less than 2, don't insert stop velocity and + // set pairs_terminal_velocity_and_accel to 0 + if (!is_driving_forward) { + *pair_itr = std::make_pair(0.0, 0.0); + continue; + } + + if (*is_driving_forward) { + for (auto & point : path_itr->points) { + // TODO(Sugahara): velocity calculation can be improved by considering the acceleration + point.point.longitudinal_velocity_mps = std::abs(point.point.longitudinal_velocity_mps); + } + // TODO(Sugahara): Consider the calculation of the target velocity and acceleration for ego's + // predicted path when ego will stop at the end of the path + *pair_itr = std::make_pair(target_velocity, acceleration); + } else { + for (auto & point : path_itr->points) { + point.point.longitudinal_velocity_mps = -std::abs(point.point.longitudinal_velocity_mps); + } + *pair_itr = std::make_pair(-target_velocity, -acceleration); + } + path_itr->points.back().point.longitudinal_velocity_mps = 0.0; + } +} + +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & start_planner_params) +{ + ego_predicted_path_params = + std::make_shared(start_planner_params->ego_predicted_path_params); +} +void updateEgoPredictedPathParams( + std::shared_ptr & ego_predicted_path_params, + const std::shared_ptr & goal_planner_params) +{ + ego_predicted_path_params = + std::make_shared(goal_planner_params->ego_predicted_path_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & start_planner_params) +{ + safety_check_params = + std::make_shared(start_planner_params->safety_check_params); +} + +void updateSafetyCheckParams( + std::shared_ptr & safety_check_params, + const std::shared_ptr & goal_planner_params) +{ + safety_check_params = + std::make_shared(goal_planner_params->safety_check_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & start_planner_params) +{ + objects_filtering_params = + std::make_shared(start_planner_params->objects_filtering_params); +} + +void updateObjectsFilteringParams( + std::shared_ptr & objects_filtering_params, + const std::shared_ptr & goal_planner_params) +{ + objects_filtering_params = + std::make_shared(goal_planner_params->objects_filtering_params); +} + +void updatePathProperty( + std::shared_ptr & ego_predicted_path_params, + const std::pair & pairs_terminal_velocity_and_accel) +{ + // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is + // necessary to ensure a reasonable path length. + // TODO(Sugahara): define them as parameter + const double min_accel_for_ego_predicted_path = 1.0; + const double acceleration = + std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); + + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; + ego_predicted_path_params->acceleration = acceleration; +} + +std::pair getPairsTerminalVelocityAndAccel( + const std::vector> & pairs_terminal_velocity_and_accel, + const size_t current_path_idx) +{ + if (pairs_terminal_velocity_and_accel.size() <= current_path_idx) { + return std::make_pair(0.0, 0.0); + } + return pairs_terminal_velocity_and_accel.at(current_path_idx); +} + +} // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index ba34901d9df6a..36fb6381ac967 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +boost::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) { const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length; @@ -86,7 +86,7 @@ boost::optional FreespacePullOut::plan(const Pose start_pose, const } // push back generate road lane path between end pose and goal pose to last path - const auto & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getGoalPose(); constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 721ffd3064840..628f1cfd7e421 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -36,7 +36,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; @@ -47,16 +47,6 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - auto lanes = road_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); @@ -69,27 +59,60 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_out_lanes); - const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_out_lane_objects, parameters_.th_moving_object_velocity); + const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); + const auto [pull_out_lane_stop_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, parameters_.collision_check_margin)) { return {}; } + const double velocity = parallel_parking_parameters_.forward_parking_velocity; if (parameters_.divide_pull_out_path) { output.partial_paths = planner_.getPaths(); + /* + Calculate the acceleration required to reach the forward parking velocity at the center of + the front path, assuming constant acceleration and deceleration. + v v + | | + | /\ | /\ + | / \ | / \ + | / \ | / \ + | / \ | / \ + |/________\_____ x |/________\______ t + 0 a_l/2 a_l 0 t_c 2*t_c + Notes: + a_l represents "arc_length_on_path_front" + t_c represents "time_to_center" + */ // insert stop velocity to first arc path end output.partial_paths.front().points.back().point.longitudinal_velocity_mps = 0.0; + const double arc_length_on_first_arc_path = + motion_utils::calcArcLength(output.partial_paths.front().points); + const double time_to_center = arc_length_on_first_arc_path / (2 * velocity); + const double average_velocity = arc_length_on_first_arc_path / (time_to_center * 2); + const double average_acceleration = average_velocity / (time_to_center * 2); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(average_velocity, average_acceleration)); + const double arc_length_on_second_arc_path = + motion_utils::calcArcLength(planner_.getArcPaths().at(1).points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / (2 * arc_length_on_second_arc_path))); } else { - auto partial_paths = planner_.getPaths(); + const auto partial_paths = planner_.getPaths(); const auto combined_path = combineReferencePath(partial_paths.at(0), partial_paths.at(1)); output.partial_paths.push_back(combined_path); + + // Calculate the acceleration required to reach the forward parking velocity at the center of + // the path, assuming constant acceleration and deceleration. + const double arc_length_on_path = motion_utils::calcArcLength(combined_path.points); + output.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(velocity, velocity * velocity / 2 * arc_length_on_path)); } + output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 08dcc0254deb1..787009299add1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -40,7 +40,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; @@ -96,17 +96,48 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) shift_path.points.begin() + collision_check_end_idx + 1); } - // check lane departure + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [&route_handler](const auto & pull_out_lane) { + return route_handler->isShoulderLanelet(pull_out_lane); + }); const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_out_lanes); + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::expandLanelets( + const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip); + dp.drivable_area_types_to_skip)); + + // crop backward path + // removes points which are out of lanes up to the start pose. + // this ensures that the backward_path stays within the drivable area when starting from a + // narrow place. + const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, + common_parameters.ego_nearest_yaw_threshold); + PathWithLaneId cropped_path{}; + for (size_t i = 0; i < shift_path.points.size(); ++i) { + const Pose pose = shift_path.points.at(i).point.pose; + const auto transformed_vehicle_footprint = + transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); + const bool is_out_of_lane = + LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); + if (i <= start_segment_idx) { + if (!is_out_of_lane) { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } else { + cropped_path.points.push_back(shift_path.points.at(i)); + } + } + shift_path.points = cropped_path.points; + + // check lane departure if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), path_start_to_end)) { + lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { continue; } @@ -163,9 +194,12 @@ std::vector ShiftPullOut::calcPullOutPaths( // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { PullOutPath non_shifted_path{}; + // In non_shifted_path, to minimize safety checks, 0 is assigned to prevent the predicted_path + // of the ego vehicle from becoming too large. non_shifted_path.partial_paths.push_back(road_lane_reference_path); non_shifted_path.start_pose = start_pose; non_shifted_path.end_pose = start_pose; + non_shifted_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(0, 0)); return non_shifted_path; }); @@ -263,6 +297,8 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } + shifted_path.path = utils::removeInverseOrderPathPoints(shifted_path.path); + // set velocity const size_t pull_out_end_idx = findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position); @@ -283,6 +319,8 @@ std::vector ShiftPullOut::calcPullOutPaths( candidate_path.partial_paths.push_back(shifted_path.path); candidate_path.start_pose = shift_line.start; candidate_path.end_pose = shift_line.end; + candidate_path.pairs_terminal_velocity_and_accel.push_back( + std::make_pair(terminal_velocity, longitudinal_acc)); candidate_paths.push_back(candidate_path); } diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 506b3ff2d634b..2945c74d51804 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1537,6 +1537,10 @@ std::vector calcBound( const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + const auto is_clockwise_polygon = + boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { return p.id() == bound.front().id(); }); @@ -1552,7 +1556,8 @@ std::vector calcBound( // extract line strings between start_idx and end_idx. const size_t start_idx = std::distance(polygon.begin(), start_itr); const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : extract_bound_from_polygon(polygon, start_idx, end_idx, is_left)) { + for (const auto & point : + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { output_points.push_back(point); } @@ -1686,8 +1691,22 @@ void makeBoundLongitudinallyMonotonic( continue; } - for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { - set_orientation(ret, j, getPose(path_points.at(i)).orientation); + if (i + 1 == path_points.size()) { + for (size_t j = intersect_idx.get(); j < bound_with_pose.size(); j++) { + if (j + 1 == bound_with_pose.size()) { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j - 1).position, bound_with_pose.at(j).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } else { + const auto yaw = + calcAzimuthAngle(bound_with_pose.at(j).position, bound_with_pose.at(j + 1).position); + set_orientation(ret, j, createQuaternionFromRPY(0.0, 0.0, yaw)); + } + } + } else { + for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { + set_orientation(ret, j, getPose(path_points.at(i)).orientation); + } } constexpr size_t OVERLAP_CHECK_NUM = 3; @@ -2284,64 +2303,126 @@ double getSignedDistanceFromBoundary( } std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side) -{ - double min_distance = std::numeric_limits::max(); - - const auto transformed_footprint = - transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); - bool found_neighbor_bound = false; - for (const auto & vehicle_corner_point : transformed_footprint) { - // convert point of footprint to pose + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side) +{ + // Depending on which side is selected, calculate the transformed coordinates of the front and + // rear vehicle corners + Point rear_corner_point, front_corner_point; + if (left_side) { + Point front_left, rear_left; + rear_left.x = -base_link2rear; + rear_left.y = vehicle_width / 2; + front_left.x = base_link2front; + front_left.y = vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_left, vehicle_pose); + } else { + Point front_right, rear_right; + rear_right.x = -base_link2rear; + rear_right.y = -vehicle_width / 2; + front_right.x = base_link2front; + front_right.y = -vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_right, vehicle_pose); + } + + const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); + const auto & bound_line_2d = left_side ? lanelet::utils::to2D(combined_lane.leftBound3d()) + : lanelet::utils::to2D(combined_lane.rightBound3d()); + + // Initialize the lateral distance to the maximum (for left side) or the minimum (for right side) + // possible value. + double lateral_distance = + left_side ? -std::numeric_limits::max() : std::numeric_limits::max(); + + // Find the closest bound segment that contains the corner point in the X-direction + // and calculate the lateral distance from that segment. + const auto calcLateralDistanceFromBound = [&]( + const Point & vehicle_corner_point, + boost::optional & lateral_distance, + boost::optional & segment_idx) { Pose vehicle_corner_pose{}; - vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.position = vehicle_corner_point; vehicle_corner_pose.orientation = vehicle_pose.orientation; - // calculate distance to the bound directly next to footprint points - lanelet::ConstLanelet closest_lanelet{}; - if (lanelet::utils::query::getClosestLanelet(lanelets, vehicle_corner_pose, &closest_lanelet)) { - const auto & bound_line_2d = left_side ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - - for (size_t i = 1; i < bound_line_2d.size(); ++i) { - const Point p_front = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i - 1]); - const Point p_back = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); - - const Point inverse_p_front = - tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); - const Point inverse_p_back = - tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); - - const double dy_front = inverse_p_front.y; - const double dy_back = inverse_p_back.y; - const double dx_front = inverse_p_front.x; - const double dx_back = inverse_p_back.x; - // is in segment - if (dx_front < 0 && dx_back > 0) { - const double lateral_distance_from_pose_to_segment = - (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); - - if (std::abs(lateral_distance_from_pose_to_segment) < std::abs(min_distance)) { - min_distance = lateral_distance_from_pose_to_segment; - } - - found_neighbor_bound = true; - break; - } + // Euclidean distance to find the closest segment containing the corner point. + double min_distance = std::numeric_limits::max(); + + for (size_t i = 0; i < bound_line_2d.size() - 1; i++) { + const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); + + const Point inverse_p1 = tier4_autoware_utils::inverseTransformPoint(p1, vehicle_corner_pose); + const Point inverse_p2 = tier4_autoware_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const double dx_p1 = inverse_p1.x; + const double dx_p2 = inverse_p2.x; + const double dy_p1 = inverse_p1.y; + const double dy_p2 = inverse_p2.y; + + // Calculate the Euclidean distances between vehicle's corner and the current and next points. + const double distance1 = tier4_autoware_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = tier4_autoware_utils::calcDistance2d(p2, vehicle_corner_point); + + // If one of the bound points is behind and the other is in front of the vehicle corner point + // and any of these points is closer than the current minimum distance, + // then update minimum distance, lateral distance and the segment index. + if (dx_p1 < 0 && dx_p2 > 0 && (distance1 < min_distance || distance2 < min_distance)) { + min_distance = std::min(distance1, distance2); + // Update lateral distance using the formula derived from similar triangles in the lateral + // cross-section view. + lateral_distance = -1.0 * (dy_p1 * dx_p2 + dy_p2 * -dx_p1) / (dx_p2 - dx_p1); + segment_idx = i; } } - } + }; - if (!found_neighbor_bound) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "neighbor shoulder bound to footprint is not found."); + // Calculate the lateral distance for both the rear and front corners of the vehicle. + boost::optional rear_segment_idx{}; + boost::optional rear_lateral_distance{}; + calcLateralDistanceFromBound(rear_corner_point, rear_lateral_distance, rear_segment_idx); + boost::optional front_segment_idx{}; + boost::optional front_lateral_distance{}; + calcLateralDistanceFromBound(front_corner_point, front_lateral_distance, front_segment_idx); + + // If no closest bound segment was found for both corners, return an empty optional. + if (!rear_lateral_distance && !front_lateral_distance) { return {}; } + // If only one of them found the closest bound, return the found lateral distance. + if (!rear_lateral_distance) { + return *front_lateral_distance; + } else if (!front_lateral_distance) { + return *rear_lateral_distance; + } + // If both corners found their closest bound, return the maximum (for left side) or the minimum + // (for right side) lateral distance. + lateral_distance = left_side ? std::max(*rear_lateral_distance, *front_lateral_distance) + : std::min(*rear_lateral_distance, *front_lateral_distance); + + // Iterate through all segments between the segments closest to the rear and front corners. + // Update the lateral distance in case any of these inner segments are closer to the vehicle. + for (size_t i = *rear_segment_idx + 1; i < *front_segment_idx; i++) { + Pose bound_pose; + bound_pose.position = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + bound_pose.orientation = vehicle_pose.orientation; + + const Point inverse_rear_point = + tier4_autoware_utils::inverseTransformPoint(rear_corner_point, bound_pose); + const Point inverse_front_point = + tier4_autoware_utils::inverseTransformPoint(front_corner_point, bound_pose); + const double dx_rear = inverse_rear_point.x; + const double dx_front = inverse_front_point.x; + const double dy_rear = inverse_rear_point.y; + const double dy_front = inverse_front_point.y; + + const double current_lateral_distance = + (dy_rear * dx_front + dy_front * -dx_rear) / (dx_front - dx_rear); + lateral_distance = left_side ? std::max(lateral_distance, current_lateral_distance) + : std::min(lateral_distance, current_lateral_distance); + } - // min_distance is the distance from corner_pose to bound, so reverse this value - return -min_distance; + return lateral_distance; } double getArcLengthToTargetLanelet( @@ -3197,4 +3278,19 @@ lanelet::ConstLanelets combineLanelets( return combined_lanes; } +PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path) +{ + PathWithLaneId fixed_path; + fixed_path.header = path.header; + fixed_path.points.push_back(path.points.at(0)); + for (size_t i = 1; i < path.points.size(); i++) { + const auto p1 = path.points.at(i - 1).point.pose; + const auto p2 = path.points.at(i).point.pose; + if (tier4_autoware_utils::isDrivingForward(p1, p2)) { + fixed_path.points.push_back(path.points.at(i)); + } + } + return fixed_path; +} + } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index e24f3274179fe..9dfcef487cc81 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -27,10 +27,10 @@ constexpr double epsilon = 1e-6; using autoware_auto_perception_msgs::msg::Shape; +using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using marker_utils::CollisionCheckDebug; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; @@ -178,18 +178,20 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) TEST(BehaviorPathPlanningSafetyUtilsTest, calcRssDistance) { using behavior_path_planner::utils::path_safety_checker::calcRssDistance; + using behavior_path_planner::utils::path_safety_checker::RSSparams; { const double front_vel = 5.0; const double front_decel = -2.0; const double rear_vel = 10.0; const double rear_decel = -1.0; - BehaviorPathPlannerParameters params; + RSSparams params; params.rear_vehicle_reaction_time = 1.0; params.rear_vehicle_safety_time_margin = 1.0; params.longitudinal_distance_min_threshold = 3.0; + params.rear_vehicle_deceleration = rear_decel; + params.front_vehicle_deceleration = front_decel; - EXPECT_NEAR( - calcRssDistance(front_vel, rear_vel, front_decel, rear_decel, params), 63.75, epsilon); + EXPECT_NEAR(calcRssDistance(front_vel, rear_vel, params), 63.75, epsilon); } } diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 6ff7104bfa992..b1e5bcb32bd18 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -4,8 +4,8 @@ common: show_processing_time: false # [-] whether to show processing time # param for input data - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal + enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: @@ -30,15 +30,30 @@ stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] # param for pass judge logic pass_judge: - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_first_margin_x: [0.0] # [[s]] time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_margin_y: [6.0] # [[s]] time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) + ego_pass_first_additional_margin: 0.5 # [s] additional time margin for ego pass first situation to suppress chattering + ego_pass_later_margin_x: [0.0] # [[s]] time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_margin_y: [10.0] # [[s]] time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) + ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering + + no_stop_decision: + max_offset_to_crosswalk_for_yield: 0.0 # [m] maximum offset from ego's front to crosswalk for yield. Positive value means in front of the crosswalk. + min_acc: -1.0 # min acceleration [m/ss] + min_jerk: -1.0 # min jerk [m/sss] + max_jerk: 1.0 # max jerk [m/sss] + stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) ## param for yielding - disable_stop_for_yield_cancel: false + disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal + disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg index 7bd11ecf2ee39..04a208eee4166 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/ttc-ttv.svg @@ -55,12 +55,12 @@
- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin
- TTV = TTC + ego_pass_later_margin + TTV = TTC + ego_pass_first_margin @@ -75,12 +75,12 @@
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin
- TTC = TTV + ego_pass_first_margin + TTC = TTV + ego_pass_later_margin @@ -160,13 +160,13 @@
- ego_pass_later_margin + ego_pass_first_margin [s]
- ego_pass_later_margin [s] + ego_pass_first_margin [s] @@ -181,13 +181,13 @@
- ego_pass_pass_margin + ego_pass_later_margin [s]
- ego_pass_pass_margin [s] + ego_pass_later_margin [s] diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index 409898233cbbe..03c61c3b7f723 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -97,7 +97,7 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.obj_polygons.size(); ++i) { const auto & points = debug_data.obj_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "object polygon", uid + i++, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + "map", now, "object polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 1.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); @@ -107,8 +107,8 @@ visualization_msgs::msg::MarkerArray createCrosswalkMarkers( for (size_t i = 0; i < debug_data.ego_polygons.size(); ++i) { const auto & points = debug_data.ego_polygons.at(i); auto marker = createDefaultMarker( - "map", now, "vehicle polygon", uid + i++, Marker::LINE_STRIP, - createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + "map", now, "vehicle polygon", uid + i, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), + createMarkerColor(1.0, 1.0, 0.0, 0.999)); marker.points = points; // marker.points.push_back(marker.points.front()); msg.markers.push_back(marker); diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index c774c8876c850..6b479736a3dad 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -66,17 +66,38 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".stuck_vehicle.max_stuck_vehicle_lateral_offset"); cp.stuck_vehicle_attention_range = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_attention_range"); + cp.min_acc_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_acc"); + cp.max_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.max_jerk"); + cp.min_jerk_for_stuck_vehicle = node.declare_parameter(ns + ".stuck_vehicle.min_jerk"); // param for pass judge logic - cp.ego_pass_first_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_first_margin"); - cp.ego_pass_later_margin = - node.declare_parameter(ns + ".pass_judge.ego_pass_later_margin"); + cp.ego_pass_first_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_x"); + cp.ego_pass_first_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_first_margin_y"); + cp.ego_pass_first_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_first_additional_margin"); + cp.ego_pass_later_margin_x = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_x"); + cp.ego_pass_later_margin_y = + node.declare_parameter>(ns + ".pass_judge.ego_pass_later_margin_y"); + cp.ego_pass_later_additional_margin = + node.declare_parameter(ns + ".pass_judge.ego_pass_later_additional_margin"); + cp.max_offset_to_crosswalk_for_yield = node.declare_parameter( + ns + ".pass_judge.no_stop_decision.max_offset_to_crosswalk_for_yield"); + cp.min_acc_for_no_stop_decision = + node.declare_parameter(ns + ".pass_judge.no_stop_decision.min_acc"); + cp.max_jerk_for_no_stop_decision = + node.declare_parameter(ns + ".pass_judge.no_stop_decision.max_jerk"); + cp.min_jerk_for_no_stop_decision = + node.declare_parameter(ns + ".pass_judge.no_stop_decision.min_jerk"); cp.stop_object_velocity = node.declare_parameter(ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = node.declare_parameter(ns + ".pass_judge.min_object_velocity"); cp.disable_stop_for_yield_cancel = node.declare_parameter(ns + ".pass_judge.disable_stop_for_yield_cancel"); + cp.disable_yield_for_new_stopped_object = + node.declare_parameter(ns + ".pass_judge.disable_yield_for_new_stopped_object"); cp.timeout_no_intention_to_walk = node.declare_parameter(ns + ".pass_judge.timeout_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index cc1cc9f1ea950..0f4c51ecfe166 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include @@ -30,10 +31,12 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; +using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; +using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; @@ -65,39 +68,34 @@ std::vector toGeometryPointVector( return points; } -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, - const geometry_msgs::msg::Polygon & base_polygon) +void offsetPolygon2d( + const geometry_msgs::msg::Pose & origin_point, const geometry_msgs::msg::Polygon & polygon, + Polygon2d & offset_polygon) { - Polygon2d one_step_polygon{}; - - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } + for (const auto & polygon_point : polygon.points) { + const auto offset_pos = + tier4_autoware_utils::calcOffsetPose(origin_point, polygon_point.x, polygon_point.y, 0.0) + .position; + offset_polygon.outer().push_back(Point2d(offset_pos.x, offset_pos.y)); } +} - { - geometry_msgs::msg::Polygon out_polygon{}; - geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); - tf2::doTransform(base_polygon, out_polygon, geometry_tf); - - for (const auto & p : out_polygon.points) { - one_step_polygon.outer().push_back(Point2d(p.x, p.y)); - } +template +Polygon2d createMultiStepPolygon( + const T & obj_path_points, const geometry_msgs::msg::Polygon & polygon, const size_t start_idx, + const size_t end_idx) +{ + Polygon2d multi_step_polygon{}; + for (size_t i = start_idx; i <= end_idx; ++i) { + offsetPolygon2d( + tier4_autoware_utils::getPose(obj_path_points.at(i)), polygon, multi_step_polygon); } - Polygon2d hull_polygon{}; - bg::convex_hull(one_step_polygon, hull_polygon); - bg::correct(hull_polygon); + Polygon2d hull_multi_step_polygon{}; + bg::convex_hull(multi_step_polygon, hull_multi_step_polygon); + bg::correct(hull_multi_step_polygon); - return hull_polygon; + return hull_multi_step_polygon; } void sortCrosswalksByDistance( @@ -124,24 +122,12 @@ void sortCrosswalksByDistance( std::sort(crosswalks.begin(), crosswalks.end(), compare); } -std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) +std::vector calcOverlappingPoints(const Polygon2d & polygon1, const Polygon2d & polygon2) { // NOTE: If one polygon is fully inside the other polygon, the result is empty. - std::vector intersection{}; - bg::intersection(polygon1, polygon2, intersection); - - if (bg::within(polygon1, polygon2)) { - for (const auto & p : polygon1.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - if (bg::within(polygon2, polygon1)) { - for (const auto & p : polygon2.outer()) { - intersection.push_back(Point2d{p.x(), p.y()}); - } - } - - return intersection; + std::vector intersection_polygons{}; + bg::intersection(polygon1, polygon2, intersection_polygons); + return intersection_polygons; } StopFactor createStopFactor( @@ -302,7 +288,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const std::optional & default_stop_pose) { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const auto & objects_ptr = planner_data_->predicted_objects; + const double ego_vel = planner_data_->current_velocity->twist.linear.x; + const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; const auto dist_ego_to_stop = calcSignedArcLength(ego_path.points, ego_pos, p_stop_line->first) + p_stop_line->second; @@ -314,45 +302,47 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range); // Update object state - updateObjectState(dist_ego_to_stop); + updateObjectState( + dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); + + // Check if ego moves forward enough to ignore yield. + const auto & p = planner_param_; + if (!path_intersects.empty()) { + const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const double dist_ego2crosswalk = + calcSignedArcLength(ego_path.points, ego_pos, path_intersects.front()); + const auto braking_distance_opt = motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, + p.min_jerk_for_no_stop_decision); + const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; + if ( + dist_ego2crosswalk - base_link2front - braking_distance < + p.max_offset_to_crosswalk_for_yield) { + return {}; + } + } // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop std::optional> nearest_stop_info; std::vector stop_factor_points; - const auto ignore_crosswalk = debug_data_.ignore_crosswalk = isRedSignalForPedestrians(); - for (const auto & object : objects_ptr->objects) { - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto obj_uuid = toHexString(object.object_id); - - if (!isCrosswalkUserType(object)) { - continue; - } - - if (ignore_crosswalk) { - continue; - } - - // NOTE: Collision points are calculated on each predicted path - const auto collision_point = - getCollisionPoints(sparse_resample_path, object, attention_area, crosswalk_attention_range); - - for (const auto & cp : collision_point) { - const auto collision_state = - getCollisionState(obj_uuid, cp.time_to_collision, cp.time_to_vehicle); - debug_data_.collision_points.push_back(std::make_pair(cp, collision_state)); - + for (const auto & object : object_info_manager_.getObject()) { + const auto & collision_point = object.collision_point; + if (collision_point) { + const auto & collision_state = object.collision_state; if (collision_state != CollisionState::YIELD) { continue; } - stop_factor_points.push_back(obj_pos); + stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength(sparse_resample_path.points, ego_pos, cp.collision_point) - + calcSignedArcLength( + sparse_resample_path.points, ego_pos, collision_point->collision_point) - planner_param_.stop_distance_from_object; if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { - nearest_stop_info = std::make_pair(cp.collision_point, dist_ego2cp - base_link2front); + nearest_stop_info = + std::make_pair(collision_point->collision_point, dist_ego2cp - base_link2front); } } } @@ -565,9 +555,9 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(clamped_near_attention_range, clamped_far_attention_range); } -std::vector CrosswalkModule::getCollisionPoints( - const PathWithLaneId & ego_path, const PredictedObject & object, const Polygon2d & attention_area, - const std::pair & crosswalk_attention_range) +std::optional CrosswalkModule::getCollisionPoint( + const PathWithLaneId & ego_path, const PredictedObject & object, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { stop_watch_.tic(__func__); @@ -581,38 +571,59 @@ std::vector CrosswalkModule::getCollisionPoints( const auto obj_polygon = createObjectPolygon(object.shape.dimensions.x, object.shape.dimensions.y); + double minimum_stop_dist = std::numeric_limits::max(); + std::optional nearest_collision_point{std::nullopt}; for (const auto & obj_path : object.kinematics.predicted_paths) { - for (size_t i = 0; i < obj_path.path.size() - 1; ++i) { - const auto & p_obj_front = obj_path.path.at(i); - const auto & p_obj_back = obj_path.path.at(i + 1); - const auto obj_one_step_polygon = createOneStepPolygon(p_obj_front, p_obj_back, obj_polygon); + size_t start_idx{0}; + bool is_start_idx_initialized{false}; + for (size_t i = 0; i < obj_path.path.size(); ++i) { + // For effective computation, the point and polygon intersection is calculated first. + const auto obj_one_step_polygon = createMultiStepPolygon(obj_path.path, obj_polygon, i, i); + const auto one_step_intersection_polygons = + calcOverlappingPoints(obj_one_step_polygon, attention_area); + if (!one_step_intersection_polygons.empty()) { + if (!is_start_idx_initialized) { + start_idx = i; + is_start_idx_initialized = true; + } + if (i != obj_path.path.size() - 1) { + // NOTE: Even if the object path does not fully cross the ego path, the path should be + // considered. + continue; + } + } - // Calculate intersection points between object and attention area - const auto tmp_intersection = calcOverlappingPoints(obj_one_step_polygon, attention_area); - if (tmp_intersection.empty()) { + if (!is_start_idx_initialized) { continue; } - // Calculate nearest collision point - double minimum_stop_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point nearest_collision_point{}; - for (const auto & p : tmp_intersection) { - const auto cp = createPoint(p.x(), p.y(), ego_pos.z); - const auto dist_ego2cp = calcSignedArcLength(ego_path.points, ego_pos, cp); + // Calculate multi-step object polygon, and reset start_idx + const size_t start_idx_with_margin = std::max(static_cast(start_idx) - 1, 0); + const size_t end_idx_with_margin = std::min(i + 1, obj_path.path.size() - 1); + const auto obj_multi_step_polygon = createMultiStepPolygon( + obj_path.path, obj_polygon, start_idx_with_margin, end_idx_with_margin); + is_start_idx_initialized = false; - if (dist_ego2cp < minimum_stop_dist) { - minimum_stop_dist = dist_ego2cp; - nearest_collision_point = cp; - } + // Calculate intersection points between object and attention area + const auto multi_step_intersection_polygons = + calcOverlappingPoints(obj_multi_step_polygon, attention_area); + if (multi_step_intersection_polygons.empty()) { + continue; } + // Calculate nearest collision point among collisions with a predicted path + Point2d boost_intersection_center_point; + bg::centroid(multi_step_intersection_polygons.front(), boost_intersection_center_point); + const auto intersection_center_point = createPoint( + boost_intersection_center_point.x(), boost_intersection_center_point.y(), ego_pos.z); + const auto dist_ego2cp = - calcSignedArcLength(ego_path.points, ego_pos, nearest_collision_point); + calcSignedArcLength(ego_path.points, ego_pos, intersection_center_point); constexpr double eps = 1e-3; const auto dist_obj2cp = calcArcLength(obj_path.path) < eps ? 0.0 - : calcSignedArcLength(obj_path.path, size_t(0), nearest_collision_point); + : calcSignedArcLength(obj_path.path, size_t(0), intersection_center_point); if ( dist_ego2cp < crosswalk_attention_range.first || @@ -620,10 +631,14 @@ std::vector CrosswalkModule::getCollisionPoints( continue; } - ret.push_back( - createCollisionPoint(nearest_collision_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel)); - - debug_data_.obj_polygons.push_back(toGeometryPointVector(obj_one_step_polygon, ego_pos.z)); + // Calculate nearest collision point among predicted paths + if (dist_ego2cp < minimum_stop_dist) { + minimum_stop_dist = dist_ego2cp; + nearest_collision_point = createCollisionPoint( + intersection_center_point, dist_ego2cp, dist_obj2cp, ego_vel, obj_vel); + debug_data_.obj_polygons.push_back( + toGeometryPointVector(obj_multi_step_polygon, ego_pos.z)); + } break; } @@ -633,7 +648,7 @@ std::vector CrosswalkModule::getCollisionPoints( logger_, planner_param_.show_processing_time, "%s : %f ms", __func__, stop_watch_.toc(__func__, true)); - return ret; + return nearest_collision_point; } CollisionPoint CrosswalkModule::createCollisionPoint( @@ -658,25 +673,6 @@ CollisionPoint CrosswalkModule::createCollisionPoint( return collision_point; } -CollisionState CrosswalkModule::getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const -{ - // First, check if the object can be ignored - const auto obj_state = object_info_manager_.getState(obj_uuid); - if (obj_state == ObjectInfo::State::FULLY_STOPPED) { - return CollisionState::IGNORE; - } - - // Compare time to collision and vehicle - if (ttc + planner_param_.ego_pass_first_margin < ttv) { - return CollisionState::EGO_PASS_FIRST; - } - if (ttv + planner_param_.ego_pass_later_margin < ttc) { - return CollisionState::EGO_PASS_LATER; - } - return CollisionState::YIELD; -} - void CrosswalkModule::applySafetySlowDownSpeed( PathWithLaneId & output, const std::vector & path_intersects) { @@ -731,15 +727,15 @@ Polygon2d CrosswalkModule::getAttentionArea( { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + const auto backward_path_length = + calcSignedArcLength(sparse_resample_path.points, size_t(0), ego_pos); + const auto length_sum = calcSignedArcLengthPartialSum( + sparse_resample_path.points, size_t(0), sparse_resample_path.points.size()); Polygon2d attention_area; for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto front_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); - const auto back_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + const auto front_length = length_sum.at(j) - backward_path_length; + const auto back_length = length_sum.at(j + 1) - backward_path_length; if (back_length < crosswalk_attention_range.first) { continue; @@ -749,7 +745,8 @@ Polygon2d CrosswalkModule::getAttentionArea( break; } - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, ego_polygon); + const auto ego_one_step_polygon = + createMultiStepPolygon(sparse_resample_path.points, ego_polygon, j, j + 1); debug_data_.ego_polygons.push_back(toGeometryPointVector(ego_one_step_polygon, ego_pos.z)); @@ -769,6 +766,8 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( const std::vector & path_intersects, const std::optional & stop_pose) const { + const auto & p = planner_param_; + if (path_intersects.size() < 2 || !stop_pose) { return {}; } @@ -779,26 +778,48 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( } const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; - if (planner_param_.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { + if (p.stuck_vehicle_velocity < std::hypot(obj_vel.x, obj_vel.y)) { continue; } const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto lateral_offset = calcLateralOffset(ego_path.points, obj_pos); - if (planner_param_.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { + if (p.max_stuck_vehicle_lateral_offset < std::abs(lateral_offset)) { continue; } const auto & ego_pos = planner_data_->current_odometry->pose.position; + const auto ego_vel = planner_data_->current_velocity->twist.linear.x; + const auto ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; + const double near_attention_range = calcSignedArcLength(ego_path.points, ego_pos, path_intersects.back()); - const double far_attention_range = - near_attention_range + planner_param_.stuck_vehicle_attention_range; + const double far_attention_range = near_attention_range + p.stuck_vehicle_attention_range; const auto dist_ego2obj = calcSignedArcLength(ego_path.points, ego_pos, obj_pos); if (near_attention_range < dist_ego2obj && dist_ego2obj < far_attention_range) { - return createStopFactor(*stop_pose, {obj_pos}); + // Plan STOP considering min_acc, max_jerk and min_jerk. + const auto min_feasible_dist_ego2stop = calcDecelDistWithJerkAndAccConstraints( + ego_vel, 0.0, ego_acc, p.min_acc_for_stuck_vehicle, p.max_jerk_for_stuck_vehicle, + p.min_jerk_for_stuck_vehicle); + if (!min_feasible_dist_ego2stop) { + continue; + } + + const double dist_ego2stop = + calcSignedArcLength(ego_path.points, ego_pos, stop_pose->position); + const double feasible_dist_ego2stop = std::max(*min_feasible_dist_ego2stop, dist_ego2stop); + const double dist_to_ego = + calcSignedArcLength(ego_path.points, ego_path.points.front().point.pose.position, ego_pos); + + const auto feasible_stop_pose = + calcLongitudinalOffsetPose(ego_path.points, 0, dist_to_ego + feasible_dist_ego2stop); + if (!feasible_stop_pose) { + continue; + } + + return createStopFactor(*feasible_stop_pose, {obj_pos}); } } @@ -835,10 +856,16 @@ std::optional CrosswalkModule::getNearestStopFactor( return {}; } -void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) +void CrosswalkModule::updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { const auto & objects_ptr = planner_data_->predicted_objects; + const auto traffic_lights_reg_elems = + crosswalk_.regulatoryElementsAs(); + const bool has_traffic_light = !traffic_lights_reg_elems.empty(); + // Check if ego is yielding const bool is_ego_yielding = [&]() { const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold; @@ -847,13 +874,34 @@ void CrosswalkModule::updateObjectState(const double dist_ego_to_stop) has_reached_stop_point; }(); + const auto ignore_crosswalk = isRedSignalForPedestrians(); + debug_data_.ignore_crosswalk = ignore_crosswalk; + // Update object state object_info_manager_.init(); for (const auto & object : objects_ptr->objects) { const auto obj_uuid = toHexString(object.object_id); + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; + + // calculate collision point and state + if (!isCrosswalkUserType(object)) { + continue; + } + if (ignore_crosswalk) { + continue; + } + + const auto collision_point = + getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( - obj_uuid, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, planner_param_); + obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, + has_traffic_light, collision_point, planner_param_); + + if (collision_point) { + const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); + debug_data_.collision_points.push_back(std::make_pair(*collision_point, collision_state)); + } } object_info_manager_.finalize(); } @@ -1003,6 +1051,9 @@ void CrosswalkModule::setDistanceToStop( void CrosswalkModule::planGo( PathWithLaneId & ego_path, const std::optional & stop_factor) const { + if (!stop_factor.has_value()) { + return; + } // Plan slow down const auto target_velocity = calcTargetVelocity(stop_factor->stop_pose.position, ego_path); insertDecelPointWithDebugInfo( diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d5b8f42dc946b..4e172344e5a70 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -56,6 +56,24 @@ using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; +namespace +{ +double interpolateEgoPassMargin( + const std::vector & x_vec, const std::vector & y_vec, const double target_x) +{ + for (size_t i = 0; i < x_vec.size(); ++i) { + if (target_x < x_vec.at(i)) { + if (i == 0) { + return y_vec.at(i); + } + const double ratio = (target_x - x_vec.at(i - 1)) / (x_vec.at(i) - x_vec.at(i - 1)); + return y_vec.at(i - 1) + ratio * (y_vec.at(i) - y_vec.at(i - 1)); + } + } + return y_vec.back(); +} +} // namespace + class CrosswalkModule : public SceneModuleInterface { public: @@ -76,12 +94,24 @@ class CrosswalkModule : public SceneModuleInterface double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; double stuck_vehicle_attention_range; + double min_acc_for_stuck_vehicle; + double max_jerk_for_stuck_vehicle; + double min_jerk_for_stuck_vehicle; // param for pass judge logic - double ego_pass_first_margin; - double ego_pass_later_margin; + std::vector ego_pass_first_margin_x; + std::vector ego_pass_first_margin_y; + double ego_pass_first_additional_margin; + std::vector ego_pass_later_margin_x; + std::vector ego_pass_later_margin_y; + double ego_pass_later_additional_margin; + double max_offset_to_crosswalk_for_yield; + double min_acc_for_no_stop_decision; + double max_jerk_for_no_stop_decision; + double min_jerk_for_no_stop_decision; double stop_object_velocity; double min_object_velocity; bool disable_stop_for_yield_cancel; + bool disable_yield_for_new_stopped_object; double timeout_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data @@ -96,19 +126,22 @@ class CrosswalkModule : public SceneModuleInterface struct ObjectInfo { - // NOTE: FULLY_STOPPED means stopped object which can be ignored. - enum class State { STOPPED = 0, FULLY_STOPPED, OTHER }; - State state{State::OTHER}; + CollisionState collision_state{}; std::optional time_to_start_stopped{std::nullopt}; - void updateState( - const rclcpp::Time & now, const double obj_vel, const bool is_ego_yielding, + geometry_msgs::msg::Point position{}; + std::optional collision_point{}; + + void transitState( + const rclcpp::Time & now, const double vel, const bool is_ego_yielding, + const bool has_traffic_light, const std::optional & collision_point, const PlannerParam & planner_param) { - const bool is_stopped = obj_vel < planner_param.stop_object_velocity; + const bool is_stopped = vel < planner_param.stop_object_velocity; + // Check if the object can be ignored if (is_stopped) { - if (state == State::FULLY_STOPPED) { + if (collision_state == CollisionState::IGNORE) { return; } @@ -117,15 +150,51 @@ class CrosswalkModule : public SceneModuleInterface } const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; - if ((is_ego_yielding || planner_param.disable_stop_for_yield_cancel) && !intent_to_cross) { - state = State::FULLY_STOPPED; - } else { - // NOTE: Object may start moving - state = State::STOPPED; + if ( + (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && + !intent_to_cross) { + collision_state = CollisionState::IGNORE; + return; } } else { time_to_start_stopped = std::nullopt; - state = State::OTHER; + } + + // Compare time to collision and vehicle + if (collision_point) { + // Check if ego will pass first + const double ego_pass_first_additional_margin = + collision_state == CollisionState::EGO_PASS_FIRST + ? 0.0 + : planner_param.ego_pass_first_additional_margin; + const double ego_pass_first_margin = interpolateEgoPassMargin( + planner_param.ego_pass_first_margin_x, planner_param.ego_pass_first_margin_y, + collision_point->time_to_collision); + if ( + collision_point->time_to_collision + ego_pass_first_margin + + ego_pass_first_additional_margin < + collision_point->time_to_vehicle) { + collision_state = CollisionState::EGO_PASS_FIRST; + return; + } + + // Check if ego will pass later + const double ego_pass_later_additional_margin = + collision_state == CollisionState::EGO_PASS_LATER + ? 0.0 + : planner_param.ego_pass_later_additional_margin; + const double ego_pass_later_margin = interpolateEgoPassMargin( + planner_param.ego_pass_later_margin_x, planner_param.ego_pass_later_margin_y, + collision_point->time_to_vehicle); + if ( + collision_point->time_to_vehicle + ego_pass_later_margin + + ego_pass_later_additional_margin < + collision_point->time_to_collision) { + collision_state = CollisionState::EGO_PASS_LATER; + return; + } + collision_state = CollisionState::YIELD; + return; } } }; @@ -133,19 +202,29 @@ class CrosswalkModule : public SceneModuleInterface { void init() { current_uuids_.clear(); } void update( - const std::string & uuid, const double obj_vel, const rclcpp::Time & now, - const bool is_ego_yielding, const PlannerParam & planner_param) + const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, + const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param) { // update current uuids current_uuids_.push_back(uuid); // add new object if (objects.count(uuid) == 0) { - objects.emplace(uuid, ObjectInfo{}); + if ( + has_traffic_light && planner_param.disable_stop_for_yield_cancel && + planner_param.disable_yield_for_new_stopped_object) { + objects.emplace(uuid, ObjectInfo{CollisionState::IGNORE}); + } else { + objects.emplace(uuid, ObjectInfo{CollisionState::YIELD}); + } } // update object state - objects.at(uuid).updateState(now, obj_vel, is_ego_yielding, planner_param); + objects.at(uuid).transitState( + now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + objects.at(uuid).collision_point = collision_point; + objects.at(uuid).position = position; } void finalize() { @@ -162,7 +241,19 @@ class CrosswalkModule : public SceneModuleInterface objects.erase(obsolete_uuid); } } - ObjectInfo::State getState(const std::string & uuid) const { return objects.at(uuid).state; } + + std::vector getObject() const + { + std::vector object_info_vec; + for (auto object : objects) { + object_info_vec.push_back(object.second); + } + return object_info_vec; + } + CollisionState getCollisionState(const std::string & uuid) const + { + return objects.at(uuid).collision_state; + } std::unordered_map objects; std::vector current_uuids_; @@ -198,9 +289,9 @@ class CrosswalkModule : public SceneModuleInterface const std::vector & path_intersects, const std::optional & stop_pose) const; - std::vector getCollisionPoints( + std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, - const Polygon2d & attention_area, const std::pair & crosswalk_attention_range); + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); std::optional getNearestStopFactor( const PathWithLaneId & ego_path, @@ -236,9 +327,6 @@ class CrosswalkModule : public SceneModuleInterface const double dist_obj2cp, const geometry_msgs::msg::Vector3 & ego_vel, const geometry_msgs::msg::Vector3 & obj_vel) const; - CollisionState getCollisionState( - const std::string & obj_uuid, const double ttc, const double ttv) const; - float calcTargetVelocity( const geometry_msgs::msg::Point & stop_point, const PathWithLaneId & ego_path) const; @@ -250,7 +338,9 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const std::vector & objects, const std::vector & path_intersects) const; - void updateObjectState(const double dist_ego_to_stop); + void updateObjectState( + const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, + const std::pair & crosswalk_attention_range, const Polygon2d & attention_area); bool isRedSignalForPedestrians() const; diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3ab642abe1e8f..e087572a1143f 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -62,7 +62,7 @@ Objects that satisfy all of the following conditions are considered as target ob #### Stuck Vehicle Detection -If there is any object in a certain distance (`stuck_vehicle_ignore_dist` and `stuck_vehicle_detect_dist`) before and after the exit of the intersection lane and the object velocity is less than a threshold (=`stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path +If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) @@ -107,8 +107,7 @@ To avoid a rapid braking, if deceleration and jerk more than a threshold (`behav | `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | | `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_ignore_dist` | double | [m] length behind the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threhsold for stuck vehicle detection | +| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | | `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | | `collision_detection.collision_start_margin_time` | double | [s] time margin for the beginning of collision with upcoming vehicle | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ce49163006d99..095073202c8fa 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -15,7 +15,6 @@ stuck_vehicle: use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning @@ -48,6 +47,7 @@ denoise_kernel: 1.0 # [m] possible_object_bbox: [1.0, 2.0] # [m x m] ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h + stop_release_margin_time: 1.0 # [s] enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval intersection: true diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index d2a8286122e54..8802dc786b617 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -5,28 +5,28 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2831px" - height="1073px" - viewBox="-0.5 -0.5 2831 1073" - content="<mxfile host="Electron" modified="2023-06-07T05:26:53.508Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="B7vxgoBz0X2z8hjEAQoV" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2842px" + height="1201px" + viewBox="-0.5 -0.5 2842 1201" + content="<mxfile host="Electron" modified="2023-08-28T08:00:31.600Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="CbgId97oWJwBUVWni98L" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - - - - - - - - - - - - - - + + + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - - - + + + - - - + + + - - - - + + + + + + + - - - - + + + + - - - + + + - - + + - + - - - - - + + + + + - +
@@ -259,22 +262,22 @@
- stuck vehicle detection area is generated on the path around the exit o... + stuck vehicle detection area is generated on the path around the exit o... - - - - - - - - - - - - + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - +
@@ -366,14 +369,14 @@
- %3CmxGraphModel%3E%3Croot%3E%3... + %3CmxGraphModel%3E%3Croot%3E%3... - - - + + + - - - + + + - - - - + + + + - - - - + + + + - - - + + + - - + + - + - - + + - - + + @@ -510,7 +513,7 @@
@@ -521,18 +524,18 @@
- stuck vehicle detection area is generated from the path, so in this... + stuck vehicle detection area is generated from the path, so in this... - - - - - + + + + + -
+
@@ -542,76 +545,158 @@
- stop_line_margin + stop_line_margin - + - - + +
`
 - ` + ` + + + + +
+
+
+ + + stuck_vehicle_detect_dist + + +
+
+
+ + stuck_vehicle_detect_dist + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + straight +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + + + + +
+
+
+ + + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%26lt%3Bfont%20style%3D%26quot%3Bfont-size%3A%2035px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3Bw%2F%20traffic%20light%2C%20right%26lt%3B%2Fb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3Bb%26gt%3B(Left-hand%20traffic)%26lt%3B%2Fb%26gt%3B%26lt%3B%2Ffont%26gt%3B%26lt%3Bspan%20style%3D%26quot%3Bfont-size%3A%2030px%3B%26quot%3B%26gt%3B%26lt%3Bb%26gt%3B%26lt%3Bbr%26gt%3B%26lt%3B%2Fb%26gt%3B%26lt%3B%2Fspan%26gt%3B%22%20style%3D%22text%3Bhtml%3D1%3Balign%3Dleft%3BverticalAlign%3Dmiddle%3Bresizable%3D0%3Bpoints%3D%5B%5D%3Bautosize%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3B%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22-17325%22%20y%3D%22-7209%22%20width%3D%22350%22%20height%3D%22140%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E + + left turn +
+
+ (Right-hand traffic) + + + +
+ + +
+
+
+ + %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%... + + diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 75c282d117e97..a039b74036954 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -40,7 +40,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & ip = intersection_param_; - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.common.attention_area_margin = node.declare_parameter(ns + ".common.attention_area_margin"); ip.common.attention_area_length = @@ -65,9 +64,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter(ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_ignore_dist = - node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_ignore_dist") + - vehicle_info.max_longitudinal_offset_m; ip.stuck_vehicle.stuck_vehicle_vel_thr = node.declare_parameter(ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); /* @@ -117,6 +113,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node.declare_parameter>(ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = node.declare_parameter(ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); + ip.occlusion.stop_release_margin_time = + node.declare_parameter(ns + ".occlusion.stop_release_margin_time"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 153f73a7b850e..f34c828db0f13 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -82,10 +82,15 @@ IntersectionModule::IntersectionModule( turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); collision_state_machine_.setMarginTime( planner_param_.collision_detection.state_transit_margin_time); - before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); - before_creep_state_machine_.setState(StateMachine::State::STOP); - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); + { + before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); + before_creep_state_machine_.setState(StateMachine::State::STOP); + } + { + occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); + occlusion_stop_state_machine_.setState(StateMachine::State::GO); + } + decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); } @@ -115,7 +120,7 @@ template void prepareRTCByDecisionResult( const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance, bool * occlusion_first_stop_required) + double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -126,8 +131,7 @@ void prepareRTCByDecisionResult( [[maybe_unused]] const IntersectionModule::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, - [[maybe_unused]] bool * occlusion_first_stop_required) + [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) { return; } @@ -136,17 +140,16 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::StuckStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "StuckStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; *default_safety = false; *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); *occlusion_safety = true; - if (!result.is_detection_area_empty) { - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + if (result.occlusion_stop_line_idx) { + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx.value(); *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); } @@ -157,15 +160,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto occlusion_stop_line = result.stop_lines.occlusion_peeking_stop_line; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + const auto occlusion_stop_line = result.occlusion_stop_line_idx; *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line); @@ -176,11 +179,10 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::FirstWaitBeforeOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; + const auto closest_idx = result.closest_idx; const auto first_stop_line_idx = result.first_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; @@ -189,7 +191,6 @@ void prepareRTCByDecisionResult( *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); - *occlusion_first_stop_required = true; return; } @@ -197,18 +198,18 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::PeekingTowardOcclusion & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; - *occlusion_safety = result.is_actually_occlusion_cleared; - *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); - const auto default_stop_line_idx = result.stop_lines.default_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); return; } @@ -216,15 +217,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto stop_line_idx = result.stop_line_idx; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -235,16 +236,15 @@ 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, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -255,16 +255,15 @@ template <> void prepareRTCByDecisionResult( const IntersectionModule::TrafficLightArrowSolidOn & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance, - [[maybe_unused]] bool * occlusion_first_stop_required) + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "TrafficLightArrowSolidOn"); - const auto closest_idx = result.stop_lines.closest_idx; - const auto default_stop_line_idx = result.stop_lines.default_stop_line; - const auto occlusion_stop_line_idx = result.stop_lines.occlusion_peeking_stop_line; + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, default_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); *occlusion_safety = true; *occlusion_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); @@ -281,11 +280,13 @@ void IntersectionModule::prepareRTCStatus( VisitorSwitch{[&](const auto & decision) { prepareRTCByDecisionResult( decision, path, &default_safety, &default_distance, &occlusion_safety_, - &occlusion_stop_distance_, &occlusion_first_stop_required_); + &occlusion_stop_distance_); }}, decision_result); setSafe(default_safety); setDistance(default_distance); + occlusion_first_stop_required_ = + std::holds_alternative(decision_result); } template @@ -326,9 +327,10 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "StuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.stuck_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -338,14 +340,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if ( - !rtc_occlusion_approved && !decision_result.is_detection_area_empty && + !rtc_occlusion_approved && decision_result.occlusion_stop_line_idx && planner_param.occlusion.enable) { - const auto occlusion_stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto occlusion_stop_line_idx = decision_result.occlusion_stop_line_idx.value(); planning_utils::setVelocityFromIndex(occlusion_stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stop_line_idx, baselink2front, *path); @@ -354,7 +356,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(closest_idx).point.pose, path->points.at(occlusion_stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -374,7 +376,7 @@ void reactRTCApprovalByDecisionResult( "NonOccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -383,12 +385,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -397,7 +399,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -426,14 +428,14 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { if (planner_param.occlusion.enable_creeping) { const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -448,7 +450,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -470,10 +472,9 @@ void reactRTCApprovalByDecisionResult( // 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_stop_line = - decision_result.stop_lines.occlusion_peeking_stop_line; + const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; if (planner_param.occlusion.enable_creeping) { - const size_t closest_idx = decision_result.stop_lines.closest_idx; + const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { planning_utils::setVelocityFromIndex( i, planner_param.occlusion.occlusion_creep_velocity, path); @@ -487,12 +488,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(occlusion_peeking_stop_line).point.pose, VelocityFactor::INTERSECTION); } } - if (!rtc_default_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -501,7 +502,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -521,7 +522,7 @@ void reactRTCApprovalByDecisionResult( "OccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_line_idx; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -530,7 +531,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -544,7 +545,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -563,7 +564,7 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "Safe, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -572,12 +573,12 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.stop_lines.occlusion_peeking_stop_line; + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -586,7 +587,7 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -606,7 +607,7 @@ void reactRTCApprovalByDecisionResult( "TrafficLightArrowSolidOn, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.stop_lines.default_stop_line; + const auto stop_line_idx = decision_result.collision_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -615,7 +616,21 @@ void reactRTCApprovalByDecisionResult( stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( - path->points, path->points.at(decision_result.stop_lines.closest_idx).point.pose, + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && planner_param.occlusion.enable) { + const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); } } @@ -639,6 +654,35 @@ void reactRTCApproval( return; } +static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) +{ + if (std::holds_alternative(decision_result)) { + return "Indecisive"; + } + if (std::holds_alternative(decision_result)) { + return "Safe"; + } + if (std::holds_alternative(decision_result)) { + return "StuckStop"; + } + 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 "TrafficLightArrowSolidOn"; + } + return ""; +} + bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_ = util::DebugData(); @@ -649,32 +693,10 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; - std::string decision_type = "intersection" + std::to_string(module_id_) + " : "; - if (std::get_if(&decision_result)) { - decision_type += "Indecisive"; - } - if (std::get_if(&decision_result)) { - decision_type += "StuckStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "NonOccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "FirstWaitBeforeOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "PeekingTowardOcclusion"; - } - if (std::get_if(&decision_result)) { - decision_type += "OccludedCollisionStop"; - } - if (std::get_if(&decision_result)) { - decision_type += "Safe"; - } - if (std::get_if(&decision_result)) { - decision_type += "TrafficLightArrowSolidOn"; - } + 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); @@ -708,13 +730,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } @@ -733,47 +753,44 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); - if (conflicting_lanelets.empty() || !first_conflicting_area) { - is_peeking_ = false; + const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); + if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { + RCLCPP_DEBUG(logger_, "conflicting area is empty"); return IntersectionModule::Indecisive{}; } + const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area = intersection_lanelets_.value().first_attention_area(); + const auto & first_attention_area_opt = intersection_lanelets_.value().first_attention_area(); const auto & dummy_first_attention_area = - first_attention_area ? first_attention_area.value() : first_conflicting_area.value(); + first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( - first_conflicting_area.value(), dummy_first_attention_area, planner_data_, - interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.stop_line_margin, planner_param_.occlusion.peeking_offset, path); + first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, + planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, + planner_param_.occlusion.peeking_offset, path); if (!intersection_stop_lines_opt) { RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto - [closest_idx, stuck_stop_line_idx, default_stop_line_idx, occlusion_peeking_stop_line_idx, - pass_judge_line_idx] = intersection_stop_lines; + [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, + occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; + const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( - lanelets_on_path, *path, associative_ids_, closest_idx, - planner_data_->vehicle_info_.vehicle_width_m); + lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, + conflicting_area, first_attention_area_opt, intersection_lanelets_.value().attention_area(), + closest_idx, planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); return IntersectionModule::Indecisive{}; } const auto path_lanelets = path_lanelets_opt.value(); - const auto ego_lane_with_next_lane = - path_lanelets.next.has_value() - ? std::vector< - lanelet::ConstLanelet>{path_lanelets.ego_or_entry2exit, path_lanelets.next.value()} - : std::vector{path_lanelets.ego_or_entry2exit}; - const bool stuck_detected = - checkStuckVehicle(planner_data_, ego_lane_with_next_lane, *path, intersection_stop_lines); + const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected) { + if (stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); const double dist_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(stuck_stop_line_idx).point.pose.position); @@ -787,24 +804,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool timeout = (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); if (!timeout) { - is_peeking_ = false; + if ( + default_stop_line_idx_opt && + motion_utils::calcSignedArcLength(path->points, stuck_stop_line_idx, closest_idx) > + planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } return IntersectionModule::StuckStop{ - stuck_stop_line_idx, !first_attention_area.has_value(), intersection_stop_lines}; + closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } } - if (!first_attention_area) { + if (!first_attention_area_opt) { RCLCPP_DEBUG(logger_, "attention area is empty"); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + const auto first_attention_area = first_attention_area_opt.value(); - if (default_stop_line_idx == 0) { - RCLCPP_DEBUG(logger_, "stop line index is 0"); - is_peeking_ = false; + if (!default_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "default stop line is null"); return IntersectionModule::Indecisive{}; } + const auto default_stop_line_idx = default_stop_line_idx_opt.value(); + // TODO(Mamoru Sobue): this part needs more formal handling const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.pass_judge_wall_pose = planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); @@ -814,24 +837,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); const double vel = std::fabs(planner_data_->current_velocity->twist.linear.x); const bool keep_detection = (vel < planner_param_.collision_detection.keep_detection_vel_thr); + const bool was_safe = std::holds_alternative(prev_decision_result_); // if ego is over the pass judge line and not stopped - if (is_peeking_) { - // do nothing - RCLCPP_DEBUG(logger_, "peeking now"); - } else if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { + if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { RCLCPP_DEBUG( logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); // do nothing } else if ( - (is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || is_permanent_go_) { + (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + is_permanent_go_) { // is_go_out_: previous RTC approval // activated_: current RTC approval is_permanent_go_ = true; RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - is_peeking_ = false; return IntersectionModule::Indecisive{}; } + if (!occlusion_peeking_stop_line_idx_opt) { + RCLCPP_DEBUG(logger_, "occlusion stop line is null"); + return IntersectionModule::Indecisive{}; + } + const auto collision_stop_line_idx = + is_over_default_stop_line ? closest_idx : default_stop_line_idx; + const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); + const auto & attention_lanelets = intersection_lanelets_.value().attention(); const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention(); @@ -865,8 +894,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (tl_arrow_solid_on) { - is_peeking_ = false; - return TrafficLightArrowSolidOn{has_collision, intersection_stop_lines}; + return TrafficLightArrowSolidOn{ + has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // check occlusion on detection lane @@ -875,6 +904,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_attention_lanelets, routing_graph_ptr, planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); } + const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / @@ -890,13 +920,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, - first_attention_area.value(), interpolated_path_info, - occlusion_attention_divisions_.value(), parked_attention_objects, occlusion_dist_thr) + first_attention_area, interpolated_path_info, occlusion_attention_divisions, + parked_attention_objects, occlusion_dist_thr) : true; + occlusion_stop_state_machine_.setStateWithMarginTime( + is_occlusion_cleared ? 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); // check safety - const bool ext_occlusion_requested = (is_occlusion_cleared && !occlusion_activated_); - if (!is_occlusion_cleared || ext_occlusion_requested) { + const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); + if ( + occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || + ext_occlusion_requested) { const double dist_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(default_stop_line_idx).point.pose.position); @@ -908,15 +945,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - if (has_collision) { - is_peeking_ = true; + if (has_collision_with_margin) { return IntersectionModule::OccludedCollisionStop{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } else { - is_peeking_ = true; return IntersectionModule::PeekingTowardOcclusion{ - occlusion_peeking_stop_line_idx, is_occlusion_cleared, intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, + occlusion_stop_line_idx}; } } else { if (is_stopped && approached_stop_line) { @@ -924,58 +960,31 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( before_creep_state_machine_.setStateWithMarginTime( StateMachine::State::GO, logger_.get_child("occlusion state_machine"), *clock_); } - is_peeking_ = true; return IntersectionModule::FirstWaitBeforeOcclusion{ - default_stop_line_idx, occlusion_peeking_stop_line_idx, is_occlusion_cleared, - intersection_stop_lines}; + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, + occlusion_stop_line_idx}; } } else if (has_collision_with_margin) { - const bool is_over_default_stopLine = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const auto stop_line_idx = is_over_default_stopLine ? closest_idx : default_stop_line_idx; - is_peeking_ = false; - return IntersectionModule::NonOccludedCollisionStop{stop_line_idx, intersection_stop_lines}; + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } - is_peeking_ = false; - return IntersectionModule::Safe{intersection_stop_lines}; + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } bool IntersectionModule::checkStuckVehicle( - const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const util::IntersectionStopLines & intersection_stop_lines) + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { const auto & objects_ptr = planner_data->predicted_objects; - const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; - const auto closest_idx = intersection_stop_lines.closest_idx; - const auto stuck_line_idx = intersection_stop_lines.stuck_stop_line; // considering lane change in the intersection, these lanelets are generated from the path - const auto ego_lane = ego_lane_with_next_lane.front(); - debug_data_.ego_lane = ego_lane.polygon3d(); const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( - input_path, ego_lane_with_next_lane, closest_idx, - planner_param_.stuck_vehicle.stuck_vehicle_detect_dist, - planner_param_.stuck_vehicle.stuck_vehicle_ignore_dist, - planner_data->vehicle_info_.vehicle_length_m); + path_lanelets, planner_param_.stuck_vehicle.stuck_vehicle_detect_dist); debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); - const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - input_path.points, input_path.points.at(stuck_line_idx).point.pose.position, - input_path.points.at(closest_idx).point.pose.position); - const bool is_over_stuck_stopline = - util::isOverTargetIndex(input_path, closest_idx, current_pose, stuck_line_idx) && - (dist_stuck_stopline > planner_param_.common.stop_overshoot_margin); - - bool is_stuck = false; - if (!is_over_stuck_stopline) { - is_stuck = util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, - &debug_data_); - } - return is_stuck; + return util::checkStuckVehicleInIntersection( + objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + &debug_data_); } autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( @@ -1054,6 +1063,7 @@ bool IntersectionModule::checkCollision( 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(); // check collision between predicted_path and ego_area @@ -1505,54 +1515,54 @@ bool IntersectionModule::isOcclusionCleared( } /* -bool IntersectionModule::checkFrontVehicleDeceleration( + 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 + 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))); + 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))); + 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)); + 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())); + 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; - } + 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); + ? 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); @@ -1566,18 +1576,18 @@ p)); const int obj_closest_centerpoint_idx = std::distance( dist_obj_center_poin 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); + 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); + tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); if (is_in_stuck_area) { - return true; + 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 9d1fab1339d0e..8ce77ea24f727 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -65,8 +65,6 @@ class IntersectionModule : public SceneModuleInterface { bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check - double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle - //! check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped /* double @@ -109,65 +107,59 @@ class IntersectionModule : public SceneModuleInterface double denoise_kernel; std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; + double stop_release_margin_time; } occlusion; }; - /* - enum OcclusionState { - NONE, - BEFORE_FIRST_STOP_LINE, - WAIT_FIRST_STOP_LINE, - CREEP_SECOND_STOP_LINE, - COLLISION_DETECTED, - }; - */ - using Indecisive = std::monostate; struct StuckStop { - size_t stop_line_idx; - // NOTE: this is optional because stuck vehicle detection is possible - // even if the detection area is empty. - // Still this may be required for RTC's default stop line - bool is_detection_area_empty; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + std::optional occlusion_stop_line_idx{std::nullopt}; }; struct NonOccludedCollisionStop { - size_t stop_line_idx; - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct FirstWaitBeforeOcclusion { - size_t first_stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t first_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct PeekingTowardOcclusion { - size_t stop_line_idx; // NOTE: if intersection_occlusion is disapproved externally through RTC, // it indicates "is_forcefully_occluded" - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct OccludedCollisionStop { - size_t stop_line_idx; - size_t occlusion_stop_line_idx; - bool is_actually_occlusion_cleared; - util::IntersectionStopLines stop_lines; + bool is_actually_occlusion_cleared{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. - util::IntersectionStopLines stop_lines; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; struct TrafficLightArrowSolidOn { - bool collision_detected; - util::IntersectionStopLines stop_lines; + bool collision_detected{false}; + size_t closest_idx{0}; + size_t collision_stop_line_idx{0}; + size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line @@ -208,14 +200,15 @@ class IntersectionModule : public SceneModuleInterface const int64_t lane_id_; const std::set associative_ids_; std::string turn_direction_; + bool is_go_out_ = false; bool is_permanent_go_ = false; - bool is_peeking_ = false; + DecisionResult prev_decision_result_; + // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; - // for an intersection lane, its associative lanes are those that share same parent lanelet and - // have same turn_direction // for occlusion detection const bool enable_occlusion_detection_; @@ -223,6 +216,7 @@ class IntersectionModule : public SceneModuleInterface // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop + StateMachine occlusion_stop_state_machine_; // NOTE: uuid_ is base member // for stuck vehicle detection @@ -235,7 +229,6 @@ class IntersectionModule : public SceneModuleInterface double occlusion_stop_distance_; bool occlusion_activated_ = true; // for first stop in two-phase stop - const UUID occlusion_first_stop_uuid_; bool occlusion_first_stop_required_ = false; void initializeRTCStatus(); @@ -246,9 +239,7 @@ class IntersectionModule : public SceneModuleInterface bool checkStuckVehicle( const std::shared_ptr & planner_data, - const lanelet::ConstLanelets & ego_lane_with_next_lane, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const util::IntersectionStopLines & intersection_stop_lines); + const util::PathLanelets & path_lanelets); autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index fe3ebeb1d95b8..17f5570341164 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -227,15 +227,20 @@ std::optional generateIntersectionStopLines( const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); // (1) default stop line position on interpolated path - int stop_idx_ip_int = 0; + bool default_stop_line_valid = true; + int stop_idx_ip_int = -1; if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data, 10.0); map_stop_idx_ip) { stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; - } else { + } + if (stop_idx_ip_int < 0) { stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - base2front_idx_dist; } + if (stop_idx_ip_int < 0) { + default_stop_line_valid = false; + } const auto default_stop_line_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; // (2) ego front stop line position on interpolated path @@ -251,13 +256,25 @@ std::optional generateIntersectionStopLines( const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); - for (size_t i = default_stop_line_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, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; + 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_stop_line_ip).point.pose; + const auto path_footprint0 = tier4_autoware_utils::transformVector( + local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); + if (bg::intersects(path_footprint0, area_2d)) { + occlusion_peeking_line_valid = false; + } + } + if (occlusion_peeking_line_valid) { + for (size_t i = default_stop_line_ip + 1; 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, area_2d)) { + occlusion_peeking_line_ip_int = i; + break; + } } } occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); @@ -279,26 +296,43 @@ std::optional generateIntersectionStopLines( // (5) stuck vehicle stop line int stuck_stop_line_ip_int = 0; + bool stuck_stop_line_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_stop_line_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); if (!stuck_stop_line_idx_ip_opt) { - return std::nullopt; + stuck_stop_line_valid = false; + stuck_stop_line_ip_int = 0; + } else { + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); } else { stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); } - const auto stuck_stop_line_ip = static_cast( - std::max(0, stuck_stop_line_ip_int - stop_line_margin_idx_dist - base2front_idx_dist)); + stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); + if (stuck_stop_line_ip_int < 0) { + stuck_stop_line_valid = false; + } + const auto stuck_stop_line_ip = static_cast(std::max(0, stuck_stop_line_ip_int)); - IntersectionStopLines intersection_stop_lines; + struct IntersectionStopLinesTemp + { + size_t closest_idx{0}; + size_t stuck_stop_line{0}; + size_t default_stop_line{0}; + size_t occlusion_peeking_stop_line{0}; + size_t pass_judge_line{0}; + }; + + IntersectionStopLinesTemp intersection_stop_lines_temp; std::list> stop_lines = { - {&closest_idx_ip, &intersection_stop_lines.closest_idx}, - {&stuck_stop_line_ip, &intersection_stop_lines.stuck_stop_line}, - {&default_stop_line_ip, &intersection_stop_lines.default_stop_line}, - {&occlusion_peeking_line_ip, &intersection_stop_lines.occlusion_peeking_stop_line}, - {&pass_judge_line_ip, &intersection_stop_lines.pass_judge_line}, + {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, + {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, + {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, + {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, + {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, }; stop_lines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); @@ -311,27 +345,64 @@ std::optional generateIntersectionStopLines( *stop_idx = insert_idx.value(); } if ( - intersection_stop_lines.occlusion_peeking_stop_line < - intersection_stop_lines.default_stop_line) { - intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines.default_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line < + intersection_stop_lines_temp.default_stop_line) { + intersection_stop_lines_temp.occlusion_peeking_stop_line = + intersection_stop_lines_temp.default_stop_line; } if ( - intersection_stop_lines.occlusion_peeking_stop_line > intersection_stop_lines.pass_judge_line) { - intersection_stop_lines.pass_judge_line = intersection_stop_lines.occlusion_peeking_stop_line; + intersection_stop_lines_temp.occlusion_peeking_stop_line > + intersection_stop_lines_temp.pass_judge_line) { + intersection_stop_lines_temp.pass_judge_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + + IntersectionStopLines intersection_stop_lines; + intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; + if (stuck_stop_line_valid) { + intersection_stop_lines.stuck_stop_line = intersection_stop_lines_temp.stuck_stop_line; } + if (default_stop_line_valid) { + intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; + } + if (occlusion_peeking_line_valid) { + intersection_stop_lines.occlusion_peeking_stop_line = + intersection_stop_lines_temp.occlusion_peeking_stop_line; + } + intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; return intersection_stop_lines; } std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon) + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward) { + // NOTE: if first point is already inside the polygon, returns nullopt const auto polygon_2d = lanelet::utils::to2D(polygon); - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - auto p = path.points.at(i).point.pose.position; - const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); - if (is_in_lanelet) { - return std::make_optional(i); + if (search_forward) { + const auto & p0 = path.points.at(lane_interval.first).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } + for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } + } + } else { + const auto & p0 = path.points.at(lane_interval.second).point.pose.position; + if (bg::within(to_bg2d(p0), polygon_2d)) { + return std::nullopt; + } + for (size_t i = lane_interval.second; i >= lane_interval.first; --i) { + const auto & p = path.points.at(i).point.pose.position; + const auto is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (is_in_lanelet) { + return std::make_optional(i); + } } } return std::nullopt; @@ -341,21 +412,39 @@ static std::optional> getFirstPointInsidePolygons( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, - const std::vector & polygons) + const std::vector & polygons, const bool search_forward = true) { - for (size_t i = lane_interval.first; i <= lane_interval.second; ++i) { - bool is_in_lanelet = false; - auto p = path.points.at(i).point.pose.position; - for (const auto & polygon : polygons) { - const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(to_bg2d(p), polygon_2d); + if (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) { - return std::make_optional>( - i, polygon); + break; } } - 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; + } } } return std::nullopt; @@ -915,35 +1004,29 @@ bool checkStuckVehicleInIntersection( } Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m) + 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; - const double extra_dist = stuck_vehicle_detect_dist + vehicle_length_m; - const double ignore_dist = stuck_vehicle_ignore_dist + vehicle_length_m; - - const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); - - const auto closest_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - - const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length - ? intersection_exit_length - ignore_dist - : closest_arc_coords.length; - - const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; + 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(ego_lane_with_next_lane, start_arc_length, end_arc_length)) - .basicPolygon(); - - Polygon2d polygon{}; + to2D(getPolygonFromArcLength(targets, 0, target_polygon_length)).basicPolygon(); if (target_polygon.empty()) { return polygon; @@ -1126,13 +1209,19 @@ static lanelet::ConstLanelets getPrevLanelets( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width) + 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) { - const auto assigned_lane_interval_opt = findLaneIdsInterval(path, associative_ids); + 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 @@ -1140,7 +1229,7 @@ std::optional generatePathLanelets( path_lanelets.all = path_lanelets.prev; // entry2ego if exist - const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval_opt.value(); + const auto [assigned_lane_start, assigned_lane_end] = assigned_lane_interval; if (closest_idx > assigned_lane_start) { path_lanelets.all.push_back( planning_utils::generatePathLanelet(path, assigned_lane_start, closest_idx, width)); @@ -1158,12 +1247,31 @@ std::optional generatePathLanelets( 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 = - planning_utils::generatePathLanelet(path, next_start + 1, next_end, width); + path_lanelets.next = planning_utils::generatePathLanelet(path, next_start, next_end, width); 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 = planning_utils::generatePathLanelet( + path, first_inside_conflicting_idx, last_inside_conflicting_idx, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(conflicting_interval)); + if (last_inside_conflicting_idx < assigned_lane_end) { + lanelet::ConstLanelet remaining_interval = planning_utils::generatePathLanelet( + path, last_inside_conflicting_idx, assigned_lane_end, width); + path_lanelets.conflicting_interval_and_remaining.push_back(std::move(remaining_interval)); + } + } return path_lanelets; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 5041336c58558..e7c77ed08451a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -83,7 +83,8 @@ std::optional generateIntersectionStopLines( std::optional getFirstPointInsidePolygon( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon); + const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, + const bool search_forward = true); /** * @brief check if ego is over the target_idx. If the index is same, compare the exact pose @@ -131,10 +132,7 @@ bool checkStuckVehicleInIntersection( DebugData * debug_data); Polygon2d generateStuckVehicleDetectAreaPolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::ConstLanelets & ego_lane_with_next_lane, const int closest_idx, - const double stuck_vehicle_detect_dist, const double stuck_vehicle_ignore_dist, - const double vehicle_length_m); + const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool checkAngleForTargetLanelets( const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, @@ -157,8 +155,12 @@ double calcDistanceUntilIntersectionLanelet( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::set & associative_ids, const size_t closest_idx, const double width); + 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); } // 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 4d2cf9695b2db..a4a0ed88217f7 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -55,10 +55,10 @@ struct DebugData struct InterpolatedPathInfo { autoware_auto_planning_msgs::msg::PathWithLaneId path; - double ds; - int lane_id; - std::set associative_lane_ids; - std::optional> lane_id_interval; + double ds{0.0}; + int lane_id{0}; + std::set associative_lane_ids{}; + std::optional> lane_id_interval{std::nullopt}; }; struct IntersectionLanelets @@ -117,19 +117,23 @@ struct IntersectionLanelets struct DescritizedLane { - int lane_id; + int lane_id{0}; // discrete fine lines from left to right - std::vector divisions; + std::vector divisions{}; }; struct IntersectionStopLines { // NOTE: for baselink - size_t closest_idx; - size_t stuck_stop_line; - size_t default_stop_line; - size_t occlusion_peeking_stop_line; - size_t pass_judge_line; + size_t closest_idx{0}; + // NOTE: null if path does not conflict with first_conflicting_area + std::optional stuck_stop_line{std::nullopt}; + // NOTE: null if path is over map stop_line OR its value is calculated negative area + std::optional default_stop_line{std::nullopt}; + // NOTE: null if footprints do not change from outside to inside of detection area + std::optional occlusion_peeking_stop_line{std::nullopt}; + // if the value is calculated negative, its value is 0 + size_t pass_judge_line{0}; }; struct PathLanelets @@ -142,6 +146,9 @@ struct PathLanelets 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::util diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 0c9b27a685583..41fe1d552b9f4 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -90,12 +90,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) "/localization/kinematic_state", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); - auto qos_transient_local = rclcpp::QoS{1}.transient_local(); + const auto durable_qos = rclcpp::QoS(1).transient_local(); vector_map_subscriber_ = create_subscription( - "input/vector_map", qos_transient_local, + "input/vector_map", durable_qos, std::bind(&MissionPlanner::onMap, this, std::placeholders::_1)); + sub_modified_goal_ = create_subscription( + "input/modified_goal", durable_qos, + std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1)); - const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -110,7 +112,6 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); - adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal); change_state(RouteState::Message::UNSET); } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 4d09f72f81899..ad8ca9cb07fbf 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -72,8 +72,10 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; Odometry::ConstSharedPtr odometry_; void on_odometry(const Odometry::ConstSharedPtr msg); + void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -124,8 +126,6 @@ class MissionPlanner : public rclcpp::Node HADMapBin::ConstSharedPtr map_ptr_{nullptr}; void onMap(const HADMapBin::ConstSharedPtr msg); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; - void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res); diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index ab2092a3120ca..f6fe3896088cf 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -129,11 +129,11 @@ double calcObstacleMaxLength(const Shape & shape) } TrajectoryPoint getExtendTrajectoryPoint( - const double extend_distance, const TrajectoryPoint & goal_point) + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = - tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0); + extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; @@ -145,6 +145,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; + const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { return output_points; @@ -154,11 +156,13 @@ std::vector extendTrajectoryPoints( double extend_sum = 0.0; while (extend_sum <= (extend_distance - step_length)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); extend_sum += step_length; } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); return output_points; diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 1aaa897986bcd..8bb897dc8ae89 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -323,7 +323,7 @@ std::vector PlannerInterface::generateStopTrajectory( // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, - abs_ego_offset); + abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); @@ -479,7 +479,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", - planner_data.current_time, i, abs_ego_offset); + planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } @@ -487,14 +487,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( if (slow_down_start_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", - planner_data.current_time, i * 2, abs_ego_offset); + planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", - planner_data.current_time, i * 2 + 1, abs_ego_offset); + planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 865350d831c4b..9c36c8bec30e7 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -98,6 +98,7 @@ class RouteHandler Pose getGoalPose() const; Pose getStartPose() const; Pose getOriginalStartPose() const; + Pose getOriginalGoalPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -121,7 +122,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -131,7 +133,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ boost::optional getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; lanelet::ConstLanelets getNextLanelets(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getPreviousLanelets(const lanelet::ConstLanelet & lanelet) const; @@ -194,7 +197,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Check if same-direction lane is available at the left side of the lanelet @@ -204,7 +208,8 @@ class RouteHandler * @return vector of lanelet having same direction if true */ lanelet::ConstLanelet getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const; + const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, + const bool get_shoulder_lane = false) const; /** * @brief Searches the furthest linestring to the right side of the lanelet @@ -302,6 +307,8 @@ class RouteHandler bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + bool getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; bool getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const; @@ -376,6 +383,7 @@ class RouteHandler // save original(not modified) route start pose for start planer execution Pose original_start_pose_; + Pose original_goal_pose_; // non-const methods void setLaneletsFromRouteMsg(); diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 238cd87db872f..b4e7be025dcaf 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -176,6 +176,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) // if get not modified route but new route, reset original start pose if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { original_start_pose_ = route_msg.start_pose; + original_goal_pose_ = route_msg.goal_pose; } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; @@ -426,13 +427,17 @@ Pose RouteHandler::getStartPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->start_pose; } Pose RouteHandler::getOriginalStartPose() const { + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalStartPose: Route has not been set yet"); + return Pose(); + } return original_start_pose_; } @@ -440,11 +445,20 @@ Pose RouteHandler::getGoalPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getGoalPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->goal_pose; } +Pose RouteHandler::getOriginalGoalPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalGoalPose: Route has not been set yet"); + return Pose(); + } + return original_goal_pose_; +} + lanelet::Id RouteHandler::getGoalLaneId() const { if (!route_ptr_ || route_ptr_->segments.empty()) { @@ -514,7 +528,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceAfter( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelet next_lanelet; if (!getNextLaneletWithinRoute(current_lanelet, &next_lanelet)) { - break; + if (only_route_lanes) { + break; + } + const auto next_lanes = getNextLanelets(current_lanelet); + if (next_lanes.empty()) { + break; + } + next_lanelet = next_lanes.front(); } // loop check if (lanelet.id() == next_lanelet.id()) { @@ -542,7 +563,14 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( while (rclcpp::ok() && length < min_length) { lanelet::ConstLanelets candidate_lanelets; if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { - break; + if (only_route_lanes) { + break; + } + const auto prev_lanes = getPreviousLanelets(current_lanelet); + if (prev_lanes.empty()) { + break; + } + candidate_lanelets = prev_lanes; } // loop check if (std::any_of( @@ -797,6 +825,13 @@ bool RouteHandler::getClosestLaneletWithinRoute( return lanelet::utils::query::getClosestLanelet(route_lanelets_, search_pose, closest_lanelet); } +bool RouteHandler::getClosestPreferredLaneletWithinRoute( + const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const +{ + return lanelet::utils::query::getClosestLanelet( + preferred_lanelets_, search_pose, closest_lanelet); +} + bool RouteHandler::getClosestLaneletWithConstrainsWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet, const double dist_threshold, const double yaw_threshold) const @@ -933,7 +968,8 @@ bool RouteHandler::isBijectiveConnection( } boost::optional RouteHandler::getRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // right road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -945,6 +981,14 @@ boost::optional RouteHandler::getRightLanelet( return boost::none; } + // right shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet right_shoulder_lanelet; + if (getRightShoulderLanelet(lanelet, &right_shoulder_lanelet)) { + return right_shoulder_lanelet; + } + } + // routable lane const auto & right_lane = routing_graph_ptr_->right(lanelet); if (right_lane) { @@ -1005,7 +1049,8 @@ bool RouteHandler::getLeftLaneletWithinRoute( } boost::optional RouteHandler::getLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // left road lanelet of shoulder lanelet if (isShoulderLanelet(lanelet)) { @@ -1017,6 +1062,14 @@ boost::optional RouteHandler::getLeftLanelet( return boost::none; } + // left shoulder lanelet + if (get_shoulder_lane) { + lanelet::ConstLanelet left_shoulder_lanelet; + if (getLeftShoulderLanelet(lanelet, &left_shoulder_lanelet)) { + return left_shoulder_lanelet; + } + } + // routable lane const auto & left_lane = routing_graph_ptr_->left(lanelet); if (left_lane) { @@ -1192,26 +1245,28 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane } lanelet::ConstLanelet RouteHandler::getMostRightLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet, enable_same_root); + const auto & same = getRightLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostRightLanelet(same.get(), enable_same_root); + return getMostRightLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; } lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const + const lanelet::ConstLanelet & lanelet, const bool enable_same_root, + const bool get_shoulder_lane) const { // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); + const auto & same = getLeftLanelet(lanelet, enable_same_root, get_shoulder_lane); if (same) { - return getMostLeftLanelet(same.get(), enable_same_root); + return getMostLeftLanelet(same.get(), enable_same_root, get_shoulder_lane); } return lanelet; diff --git a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp index 7c302eec20431..5829277335433 100644 --- a/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/downsample_filter/faster_voxel_grid_downsample_filter.cpp @@ -36,7 +36,12 @@ void FasterVoxelGridDownsampleFilter::set_field_offsets(const PointCloud2ConstPt x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; - intensity_offset_ = input->fields[pcl::getFieldIndex(*input, "intensity")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } offset_initialized_ = true; }