diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 091f3405e2815..1a800a38d1051 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -109,13 +109,16 @@ std::vector resamplePointVector( } std::vector resamplePoseVector( - const std::vector & points, + const std::vector & points_raw, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { + // Remove overlap points for resampling + const auto points = motion_utils::removeOverlapPoints(points_raw); + // validate arguments if (!resample_utils::validate_arguments(points, resampled_arclength)) { - return points; + return points_raw; } std::vector position(points.size()); 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/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 c74c0fdcc63b7..eab2a56607dee 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 @@ -79,12 +79,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -96,6 +138,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 99f09cfe6abbd..463340efdecfe 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 @@ -143,12 +143,54 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/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_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_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 1bc351b08c01b..a540688f7e751 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -422,4 +422,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 77f6a5c506d92..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 @@ -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/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 6ba90f54e48fe..a6f9565c08aae 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 @@ -81,13 +81,16 @@ struct PullOverStatus size_t current_path_idx{0}; bool require_increment_{true}; // if false, keep current path idx. std::shared_ptr prev_stop_path{nullptr}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; + // stop path after approval, stop path is not updated until safety is confirmed 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 has_decided_path{false}; // if true, the path has 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 prev_is_safe_dynamic_objects{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; bool is_ready{false}; @@ -230,6 +233,18 @@ class GoalPlannerModule : public SceneModuleInterface const Pose & search_start_offset_pose, PathWithLaneId & path) const; PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); + + /** + * @brief Generate a stop point in the current path based on the vehicle's pose and constraints. + * + * This function generates a stop point in the current path. If no lanes are available or if + * stopping is not feasible due to constraints (maximum deceleration, maximum jerk), no stop point + * is inserted into the path. + * + * @return the modified path with the stop point inserted. If no feasible stop point can be + * determined, returns an empty optional. + */ + std::optional generateStopInsertedCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -277,6 +292,17 @@ class GoalPlannerModule : public SceneModuleInterface // output setter void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output); + + /** + * @brief Sets a stop path in the current path based on safety conditions and previous paths. + * + * This function sets a stop path in the current path. Depending on whether the previous safety + * judgement against dynamic objects were safe or if a previous stop path existed, it either + * generates a new stop path or uses the previous stop path. + * + * @param output BehaviorModuleOutput + */ + void setStopPathFromCurrentPath(BehaviorModuleOutput & output); void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; 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 771fbd93f7196..4c095a38e7f73 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 @@ -83,7 +83,7 @@ struct StartPlannerParameters double maximum_jerk_for_stop; // hysteresis parameter - double hysteresis_factor_expand_rate; + double hysteresis_factor_expand_rate = 0.0; // path safety checker utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; 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 54a052c91f180..12f6f243e9a9c 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 @@ -664,18 +664,25 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { if (status_.is_safe_static_objects) { - // clear stop pose when the path is safe and activated - if (isActivated()) { - resetWallPoses(); + if (isSafePath()) { + // clear stop pose when the path is safe against static/dynamic objects and activated + if (isActivated()) { + resetWallPoses(); + } + // keep stop if not enough time passed, + // because it takes time for the trajectory to be reflected + auto current_path = getCurrentPath(); + keepStoppedWithCurrentPath(current_path); + + output.path = std::make_shared(current_path); + output.reference_path = getPreviousModuleOutput().reference_path; + } else if (status_.has_decided_path && isActivated()) { + // situation : not safe against dynamic objects after approval + // insert stop point in current path if ego is able to stop with acceleration and jerk + // constraints + setStopPathFromCurrentPath(output); } - // keep stop if not enough time passed, - // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); - keepStoppedWithCurrentPath(current_path); - - output.path = std::make_shared(current_path); - output.reference_path = getPreviousModuleOutput().reference_path; } else { // not safe: use stop_path setStopPath(output); @@ -693,6 +700,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_static_objects; + status_.prev_is_safe_dynamic_objects = status_.is_safe_dynamic_objects; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -720,6 +728,30 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) output.reference_path = getPreviousModuleOutput().reference_path; } +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) +{ + // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path + if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { + if (const auto stop_inserted_path = generateStopInsertedCurrentPath()) { + output.path = std::make_shared(*stop_inserted_path); + status_.prev_stop_path_after_approval = output.path; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); + } else { + output.path = std::make_shared(getCurrentPath()); + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, + "Collision detected, no feasible stop path found, cannot stop."); + } + std::lock_guard lock(mutex_); + last_path_update_time_ = std::make_unique(clock_->now()); + } else { + // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path + output.path = status_.prev_stop_path_after_approval; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); + } + output.reference_path = getPreviousModuleOutput().reference_path; +} + void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { @@ -1079,6 +1111,34 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } +std::optional GoalPlannerModule::generateStopInsertedCurrentPath() +{ + auto current_path = getCurrentPath(); + if (current_path.points.empty()) { + return {}; + } + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + if (!min_stop_distance) { + return {}; + } + + // set stop point + const auto stop_idx = motion_utils::insertStopPoint( + planner_data_->self_odometry->pose.pose, *min_stop_distance, current_path.points); + + if (!stop_idx) { + return {}; + } else { + stop_pose_ = current_path.points.at(*stop_idx).point.pose; + } + + return current_path; +} + void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { if (isActivated() && last_approved_time_ != nullptr) { 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 29e16fd570720..a285224404f12 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 @@ -690,9 +690,16 @@ 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); + // 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( start_pose_candidates.points, current_pose.position, -back_distance); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 617d4a224fdd5..d6af88f39a8d2 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2851,7 +2851,7 @@ BehaviorModuleOutput getReferencePath( // clip backward length // NOTE: In order to keep backward_path_length at least, resampling interval is added to the // backward. - const size_t current_seg_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( reference_path.points, no_shift_pose, p.ego_nearest_dist_threshold, p.ego_nearest_yaw_threshold); reference_path.points = motion_utils::cropPoints( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 9680117d726dd..5f9d838b7c220 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -125,7 +125,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; @@ -136,8 +136,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; } @@ -146,17 +145,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); } @@ -167,15 +165,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); @@ -186,11 +184,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; @@ -199,7 +196,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; } @@ -207,18 +203,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; } @@ -226,15 +222,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); @@ -245,16 +241,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); @@ -265,16 +260,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); @@ -291,11 +285,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 @@ -336,16 +332,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.stop_lines.closest_idx; + const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - auto stop_line_idx = decision_result.stop_line_idx; - if ( - !decision_result.is_detection_area_empty && - motion_utils::calcSignedArcLength(path->points, stop_line_idx, closest_idx) > - planner_param.common.stop_overshoot_margin) { - stop_line_idx = decision_result.stop_lines.default_stop_line; - } + 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); @@ -360,9 +350,9 @@ void reactRTCApprovalByDecisionResult( } } 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); @@ -391,7 +381,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); @@ -400,12 +390,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); @@ -414,7 +404,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); } } @@ -443,14 +433,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); @@ -465,7 +455,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); } } @@ -487,10 +477,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); @@ -504,12 +493,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); @@ -518,7 +507,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); } } @@ -538,7 +527,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); @@ -547,7 +536,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); } } @@ -561,7 +550,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); } } @@ -580,7 +569,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); @@ -589,12 +578,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); @@ -603,7 +592,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); } } @@ -623,7 +612,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); @@ -632,7 +621,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); } } @@ -695,6 +698,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * // calculate the const auto decision_result = modifyPathVelocityDetail(path, stop_reason); + prev_decision_result_ = decision_result; const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); @@ -731,13 +735,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{}; } @@ -756,33 +758,33 @@ 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, interpolated_path_info, associative_ids_, first_conflicting_area.value(), - conflicting_area, first_attention_area, intersection_lanelets_.value().attention_area(), + 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"); @@ -790,10 +792,10 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } const auto path_lanelets = path_lanelets_opt.value(); - const bool stuck_detected = checkStuckVehicle( - planner_data_, path_lanelets, interpolated_path_info, 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); @@ -807,24 +809,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); @@ -837,24 +845,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_data_->current_velocity->twist.linear.y); const bool keep_detection = (vel_norm < 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(); @@ -888,8 +902,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 @@ -898,6 +912,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) / @@ -915,15 +930,17 @@ 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_); + const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); if ( occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || ext_occlusion_requested) { @@ -939,68 +956,45 @@ 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) { // start waiting at the first stop line before_creep_state_machine_.setState(StateMachine::State::GO); } - 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 util::PathLanelets & path_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, - 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 & path = interpolated_path_info.path; const auto stuck_vehicle_detect_area = util::generateStuckVehicleDetectAreaPolygon( 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( - path.points, path.points.at(stuck_line_idx).point.pose.position, - path.points.at(closest_idx).point.pose.position); - const bool is_over_stuck_stopline = - util::isOverTargetIndex(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( @@ -1531,54 +1525,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); @@ -1592,18 +1586,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 8da150cc3bc90..8063300b9b787 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -111,62 +111,55 @@ class IntersectionModule : public SceneModuleInterface } 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 @@ -207,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_; @@ -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 util::PathLanelets & path_lanelets, - const util::InterpolatedPathInfo & interpolated_path_info, - 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 940513f8fe7c3..ce2d7252ac93d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -228,15 +228,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 @@ -252,13 +257,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); @@ -280,26 +297,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)); }); @@ -312,14 +346,31 @@ 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; } @@ -328,8 +379,13 @@ std::optional getFirstPointInsidePolygon( 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); 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); @@ -338,6 +394,10 @@ std::optional getFirstPointInsidePolygon( } } } 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); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 0af797b35b2cc..64be330a99232 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 DiscretizedLane { - 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 diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 08ebea4284bf1..f3f1932c44c43 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -20,7 +20,11 @@ nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] - suppress_sudden_obstacle_stop: false + stop_on_curve: + enable_approaching: true # false + additional_safe_distance_margin: 0.0 # [m] + min_safe_distance_margin: 3.0 # [m] + suppress_sudden_obstacle_stop: true stop_obstacle_type: unknown: true @@ -54,7 +58,7 @@ pedestrian: false slow_down_obstacle_type: - unknown: true + unknown: false car: true truck: true bus: true diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 9b7d318879e5f..e958074971f2a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -108,12 +108,15 @@ struct StopObstacle : public TargetObstacleInterface { StopObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, - const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point) + const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, + const double arg_lon_velocity, const double arg_lat_velocity, + const geometry_msgs::msg::Point arg_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), + shape(arg_shape), collision_point(arg_collision_point) { } + Shape shape; geometry_msgs::msg::Point collision_point; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index ac6684d163aea..00afc11985d72 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -100,6 +100,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; std::vector stop_obstacle_types_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 8d67a89e369e9..9a5a6521a6494 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -54,11 +54,16 @@ class PlannerInterface void setParam( const bool enable_debug_info, const bool enable_calculation_time_info, - const double min_behavior_stop_margin, const bool suppress_sudden_obstacle_stop) + const double min_behavior_stop_margin, const double enable_approaching_on_curve, + const double additional_safe_distance_margin_on_curve, + const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) { enable_debug_info_ = enable_debug_info; enable_calculation_time_info_ = enable_calculation_time_info; min_behavior_stop_margin_ = min_behavior_stop_margin; + enable_approaching_on_curve_ = enable_approaching_on_curve; + additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; + min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve; suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop; } @@ -103,6 +108,9 @@ class PlannerInterface bool enable_calculation_time_info_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; bool suppress_sudden_obstacle_stop_; // stop watch @@ -195,6 +203,8 @@ class PlannerInterface std::optional start_point{std::nullopt}; std::optional end_point{std::nullopt}; }; + double calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const; double calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const; std::optional> calculateDistanceToSlowDownWithConstraints( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index fc454e1ed6426..14324709ba9e1 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -398,11 +398,18 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); + additional_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.additional_safe_distance_margin"); + enable_approaching_on_curve_ = + declare_parameter("common.stop_on_curve.enable_approaching"); + min_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.min_safe_distance_margin"); suppress_sudden_obstacle_stop_ = declare_parameter("common.suppress_sudden_obstacle_stop"); planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, - suppress_sudden_obstacle_stop_); + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); } { // stop/cruise/slow down obstacle type @@ -439,9 +446,20 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( parameters, "common.enable_debug_info", enable_debug_info_); tier4_autoware_utils::updateParam( parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); + + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.additional_safe_distance_margin", + additional_safe_distance_margin_on_curve_); + tier4_autoware_utils::updateParam( + parameters, "common.stop_on_curve.min_safe_distance_margin", + min_safe_distance_margin_on_curve_); + planner_ptr_->setParam( enable_debug_info_, enable_calculation_time_info_, min_behavior_stop_margin_, - suppress_sudden_obstacle_stop_); + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); tier4_autoware_utils::updateParam( parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); @@ -912,7 +930,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, + return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, tangent_vel, normal_vel, *collision_point}; } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index f4f729f1fe8b0..5ea637655a01a 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -16,6 +16,9 @@ #include "motion_utils/distance/distance.hpp" #include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/resample/resample.hpp" +#include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" @@ -206,6 +209,19 @@ double calcDecelerationVelocityFromDistanceToTarget( } return current_velocity; } + +std::vector resampleTrajectoryPoints( + const std::vector & traj_points, const double interval) +{ + const auto traj = motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = motion_utils::resampleTrajectory(traj, interval); + return motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +tier4_autoware_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +{ + return tier4_autoware_utils::Point2d{p.x, p.y}; +} } // namespace std::vector PlannerInterface::generateStopTrajectory( @@ -261,16 +277,19 @@ std::vector PlannerInterface::generateStopTrajectory( planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, 0); const double dist_to_ego = -negative_dist_to_ego; + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const double margin_from_obstacle = [&]() { + const double margin_from_obstacle_considering_behavior_module = [&]() { const size_t nearest_segment_idx = findEgoSegmentIndex(planner_data.traj_points, planner_data.ego_pose); const auto closest_behavior_stop_idx = motion_utils::searchZeroVelocityIndex(planner_data.traj_points, nearest_segment_idx + 1); if (!closest_behavior_stop_idx) { - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; } const double closest_behavior_stop_dist_from_ego = motion_utils::calcSignedArcLength( @@ -284,29 +303,28 @@ std::vector PlannerInterface::generateStopTrajectory( abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (stop_dist_diff < margin_from_obstacle) { // Use terminal margin (terminal_safe_distance_margin) for obstacle stop return longitudinal_info_.terminal_safe_distance_margin; } } else { - const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego - - longitudinal_info_.safe_distance_margin - - abs_ego_offset; + const double closest_obstacle_stop_dist_from_ego = + closest_obstacle_dist - dist_to_ego - margin_from_obstacle - abs_ego_offset; const double stop_dist_diff = closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; - if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) { + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { // Use shorter margin (min_behavior_stop_margin) for obstacle stop return min_behavior_stop_margin_; } } - return longitudinal_info_.safe_distance_margin; + return margin_from_obstacle; }(); const auto [stop_margin_from_obstacle, will_collide_with_obstacle] = [&]() { // Check feasibility to stop if (suppress_sudden_obstacle_stop_) { const double closest_obstacle_stop_dist = - closest_obstacle_dist - margin_from_obstacle - abs_ego_offset; + closest_obstacle_dist - margin_from_obstacle_considering_behavior_module - abs_ego_offset; // Calculate feasible stop margin (Check the feasibility) const double feasible_stop_dist = calcMinimumDistanceToStop( @@ -316,11 +334,12 @@ std::vector PlannerInterface::generateStopTrajectory( if (closest_obstacle_stop_dist < feasible_stop_dist) { const auto feasible_margin_from_obstacle = - margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist); + margin_from_obstacle_considering_behavior_module - + (feasible_stop_dist - closest_obstacle_stop_dist); return std::make_pair(feasible_margin_from_obstacle, true); } } - return std::make_pair(margin_from_obstacle, false); + return std::make_pair(margin_from_obstacle_considering_behavior_module, false); }(); // Generate Output Trajectory @@ -397,6 +416,91 @@ std::vector PlannerInterface::generateStopTrajectory( return output_traj_points; } +double PlannerInterface::calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const +{ + if (!enable_approaching_on_curve_) { + return longitudinal_info_.safe_distance_margin; + } + + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + // calculate short trajectory points towards obstacle + const size_t obj_segment_idx = + motion_utils::findNearestSegmentIndex(planner_data.traj_points, stop_obstacle.collision_point); + std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; + double sum_short_traj_length{0.0}; + for (int i = obj_segment_idx; 0 <= i; --i) { + short_traj_points.push_back(planner_data.traj_points.at(i)); + + if ( + 1 < short_traj_points.size() && + longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { + break; + } + sum_short_traj_length += tier4_autoware_utils::calcDistance2d( + planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); + } + std::reverse(short_traj_points.begin(), short_traj_points.end()); + if (short_traj_points.size() < 2) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate collision index between straight line from ego pose and object + const auto calculate_distance_from_straight_ego_path = + [&](const auto & ego_pose, const auto & object_polygon) { + const auto forward_ego_pose = tier4_autoware_utils::calcOffsetPose( + ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); + const auto ego_straight_segment = tier4_autoware_utils::Segment2d{ + convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; + return boost::geometry::distance(ego_straight_segment, object_polygon); + }; + const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); + const auto object_polygon = + tier4_autoware_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto collision_idx = [&]() -> std::optional { + for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { + const double dist_to_obj = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(i).pose, object_polygon); + if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { + return i; + } + } + return std::nullopt; + }(); + if (!collision_idx) { + return min_safe_distance_margin_on_curve_; + } + if (*collision_idx == 0) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate margin from obstacle + const double partial_segment_length = [&]() { + const double collision_segment_length = tier4_autoware_utils::calcDistance2d( + resampled_short_traj_points.at(*collision_idx - 1), + resampled_short_traj_points.at(*collision_idx)); + const double prev_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); + const double next_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx).pose, object_polygon); + return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) * + collision_segment_length; + }(); + + const double short_margin_from_obstacle = + partial_segment_length + + motion_utils::calcSignedArcLength( + resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - + abs_ego_offset + additional_safe_distance_margin_on_curve_; + + return std::min( + longitudinal_info_.safe_distance_margin, + std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); +} + double PlannerInterface::calcDistanceToCollisionPoint( const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point) {