diff --git a/.cspell-partial.json b/.cspell-partial.json index 45b85e779c39d..f45ae8961c56b 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -1,5 +1,9 @@ { - "ignorePaths": ["**/perception/**", "**/sensing/**", "perception/bytetrack/lib/**"], + "ignorePaths": [ + "**/perception/**", + "sensing/tier4_pcl_extensions/include/**", + "perception/bytetrack/lib/**" + ], "ignoreRegExpList": [], "words": [] } diff --git a/common/perception_utils/package.xml b/common/perception_utils/package.xml index 6cae7f9ce8c4f..5304d481737e5 100644 --- a/common/perception_utils/package.xml +++ b/common/perception_utils/package.xml @@ -4,7 +4,6 @@ perception_utils 0.1.0 The perception_utils package - Mingyu Li Satoshi Tanaka Yusuke Muramatsu Shunsuke Miura diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index a17e491ebe98d..5062c596dce00 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -122,14 +122,24 @@ bool AutonomousMode::isModeChangeCompleted() const auto closest_point = trajectory_.points.at(*closest_idx); // check for lateral deviation - const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const auto dist_deviation = + motion_utils::calcLateralOffset(trajectory_.points, kinematics_.pose.pose.position); + if (std::isnan(dist_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (dist_deviation > stable_check_param_.dist_threshold) { RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation); return unstable(); } // check for yaw deviation - const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const auto yaw_deviation = + motion_utils::calcYawDeviation(trajectory_.points, kinematics_.pose.pose); + if (std::isnan(yaw_deviation)) { + RCLCPP_INFO(logger_, "Not stable yet: lateral offset calculation failed."); + return unstable(); + } if (yaw_deviation > stable_check_param_.yaw_threshold) { RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); return unstable(); diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 667fd59196df3..154fc8526f512 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -270,7 +270,8 @@ void NDTScanMatcher::timer_diagnostic() } if ( state_ptr_->count("skipping_publish_num") && - std::stoi((*state_ptr_)["skipping_publish_num"]) > 1) { + std::stoi((*state_ptr_)["skipping_publish_num"]) > 1 && + std::stoi((*state_ptr_)["skipping_publish_num"]) < 5) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "skipping_publish_num > 1. "; } diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 3b4855f1f38e9..161a4d19b96ef 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -19,7 +19,8 @@ #include #include #include -#include +#include +#include #include #include diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml index f029972f7124f..bec5b666683c4 100644 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ b/perception/crosswalk_traffic_light_estimator/package.xml @@ -5,6 +5,8 @@ The crosswalk_traffic_light_estimator package Satoshi Ota + Shunsuke Miura + Tao Zhong Apache License 2.0 ament_cmake_auto diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp index 97c4387122301..9ef1f75427b65 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp @@ -16,7 +16,6 @@ #define OBSTACLE_POINTCLOUD_BASED_VALIDATOR__DEBUGGER_HPP_ #include -#include #include #include diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index d22a05fc7162d..2e393f27c37a7 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -15,7 +15,7 @@ #include "detected_object_filter/object_lanelet_filter.hpp" #include -#include +#include #include #include diff --git a/perception/detected_object_validation/src/object_position_filter.cpp b/perception/detected_object_validation/src/object_position_filter.cpp index 6980f5fd3c17f..0f59e60d57d55 100644 --- a/perception/detected_object_validation/src/object_position_filter.cpp +++ b/perception/detected_object_validation/src/object_position_filter.cpp @@ -14,8 +14,6 @@ #include "detected_object_filter/object_position_filter.hpp" -#include - namespace object_position_filter { ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & node_options) diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index bad20d8da577f..00e53e9de9a9c 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -15,7 +15,7 @@ #include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp" #include -#include +#include #include diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp index 50e3f3b540929..948f040d7ebde 100644 --- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp +++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp index 09ae73a7fa03c..fb7642356e8d9 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/debugger.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 577e0ff12c367..0eacfa527750b 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index de71f94151486..7b436e1edd64c 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -16,6 +16,9 @@ #include "object_recognition_utils/object_recognition_utils.hpp" +#include +#include + #include #include #include diff --git a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp index 5141bcad04990..01f738fc6beaa 100644 --- a/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp +++ b/perception/front_vehicle_velocity_estimator/include/front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp @@ -19,7 +19,7 @@ #include "pcl/point_types.h" #include "pcl_conversions/pcl_conversions.h" #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "nav_msgs/msg/odometry.hpp" diff --git a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp index f1b8be19c3223..6f59823615c04 100644 --- a/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp +++ b/perception/front_vehicle_velocity_estimator/src/front_vehicle_velocity_estimator_node/front_vehicle_velocity_estimator.cpp @@ -14,6 +14,8 @@ #include "front_vehicle_velocity_estimator/front_vehicle_velocity_estimator.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + #include #include diff --git a/perception/heatmap_visualizer/src/utils.cpp b/perception/heatmap_visualizer/src/utils.cpp index 2c1be762b2cc9..c4b1c3d6a22c6 100644 --- a/perception/heatmap_visualizer/src/utils.cpp +++ b/perception/heatmap_visualizer/src/utils.cpp @@ -15,7 +15,8 @@ #include "heatmap_visualizer/utils.hpp" #include -#include +#include +#include #include diff --git a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp index 90129b837eb1b..d4d7b1379776a 100644 --- a/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/lidar_centerpoint/lib/postprocess/non_maximum_suppression.cpp @@ -16,8 +16,7 @@ #include "object_recognition_utils/geometry.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/geometry.hpp" namespace centerpoint { diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index f115d8c07f72f..d2b6ee5de475e 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -21,14 +21,16 @@ #include #include -#include #include #include #include #include +#include #include +#include + #include #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 006a87430965f..6571e70b8c123 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -20,7 +20,9 @@ #include "multi_object_tracker/utils/utils.hpp" -#include +#include +#include +#include #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 50f582d6a2501..97d6d48c35d1b 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -34,7 +34,9 @@ #include #include -#include +#include +#include +#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index d9137bc67ef55..51adca7e69b56 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -18,8 +18,6 @@ #include "multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp" -#include - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index 978b5e346efb2..aa3f7b1c30d01 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -34,7 +34,9 @@ #include #include -#include +#include +#include +#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; diff --git a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp index 00822a2776ec8..2084ac28e70f0 100644 --- a/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pass_through_tracker.cpp @@ -34,7 +34,6 @@ #include #include -#include PassThroughTracker::PassThroughTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index 87036c9b7665a..eed9d05359b77 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -18,8 +18,6 @@ #include "multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp" -#include - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index d2f144a25f103..b168717042db3 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -21,7 +21,9 @@ #include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include +#include +#include +#include #include #include diff --git a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp index 474191f8926ec..897b858a6aabe 100644 --- a/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/unknown_tracker.cpp @@ -30,7 +30,9 @@ #include #include -#include +#include +#include +#include UnknownTracker::UnknownTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, diff --git a/perception/object_merger/include/object_association_merger/utils/utils.hpp b/perception/object_merger/include/object_association_merger/utils/utils.hpp index dce5a48c5d2ce..bb4466bd4944d 100644 --- a/perception/object_merger/include/object_association_merger/utils/utils.hpp +++ b/perception/object_merger/include/object_association_merger/utils/utils.hpp @@ -19,8 +19,6 @@ #ifndef OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ #define OBJECT_ASSOCIATION_MERGER__UTILS__UTILS_HPP_ -#include - #include #include #include diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 4f600ce8a4948..0ad2a76f74754 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -16,7 +16,7 @@ #include "object_association_merger/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp index 775f612b8caeb..a5d54b63b3311 100644 --- a/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp +++ b/perception/object_velocity_splitter/src/object_velocity_splitter_node/object_velocity_splitter_node.cpp @@ -14,7 +14,7 @@ #include "object_velocity_splitter/object_velocity_splitter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 72136c20fddb8..691c13a6d4701 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -15,7 +15,9 @@ #include "occupancy_grid_map_outlier_filter/occupancy_grid_map_outlier_filter_nodelet.hpp" #include -#include +#include +#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp index 4af7ef950272d..047b747c2861f 100644 --- a/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/utils/utils.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index e6f8ca17757c2..b4505eedddd21 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -18,7 +18,6 @@ #include "utils/utils.hpp" #include -#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp index 4e71c4bda3f21..8652cfa34d96c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_base.cpp @@ -56,7 +56,6 @@ #include #include -#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp index b07076de8342a..56aeea30e0773 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_fixed.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp index 9556b0a93cc93..20a5770e37fdb 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map_projective.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index e8b53b91d81f7..f6369602b8890 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -20,7 +20,8 @@ #include "utils/utils.hpp" #include -#include +#include +#include #include diff --git a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp index f2cd6203b3685..0060754cd875c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/utils/utils.cpp @@ -14,6 +14,8 @@ #include "utils/utils.hpp" +#include + #include namespace utils diff --git a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp index e51024ba4e205..b5f2005a84baf 100644 --- a/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp +++ b/perception/radar_fusion_to_detected_object/include/radar_fusion_to_detected_object.hpp @@ -16,8 +16,7 @@ #define RADAR_FUSION_TO_DETECTED_OBJECT_HPP_ #include "rclcpp/logger.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" #define EIGEN_MPL2_ONLY #include #include diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 79e6db0a69a71..839a606c0e307 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -15,6 +15,9 @@ #include "radar_fusion_to_detected_object.hpp" +#include +#include + #include #include diff --git a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp index 0e82c5388d297..c762996f8e0a7 100644 --- a/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp +++ b/perception/radar_object_clustering/include/radar_object_clustering/radar_object_clustering_node.hpp @@ -16,7 +16,6 @@ #define RADAR_OBJECT_CLUSTERING__RADAR_OBJECT_CLUSTERING_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp index 7f9879fc357a5..74e85bde21385 100644 --- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp +++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp @@ -15,6 +15,8 @@ #include "radar_object_clustering/radar_object_clustering_node.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" #include diff --git a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp index 62ebb2ec0146b..351410161d8b2 100644 --- a/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp +++ b/perception/radar_tracks_msgs_converter/include/radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp @@ -16,7 +16,7 @@ #define RADAR_TRACKS_MSGS_CONVERTER__RADAR_TRACKS_MSGS_CONVERTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" #include "autoware_auto_perception_msgs/msg/object_classification.hpp" diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index ab5e4eb5abe90..75556fbb0c372 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -14,7 +14,9 @@ #include "radar_tracks_msgs_converter/radar_tracks_msgs_converter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include diff --git a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp index b932c67133850..60ccb0b4ef02c 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/utils.hpp @@ -17,8 +17,6 @@ #include "shape_estimation/shape_estimator.hpp" -#include - #include #include diff --git a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp index 7f9baa9a0f5e0..ccd04bae40698 100644 --- a/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp +++ b/perception/shape_estimation/include/shape_estimation/model/model_interface.hpp @@ -15,8 +15,6 @@ #ifndef SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ #define SHAPE_ESTIMATION__MODEL__MODEL_INTERFACE_HPP_ -#include - #include #include diff --git a/perception/shape_estimation/lib/corrector/utils.cpp b/perception/shape_estimation/lib/corrector/utils.cpp index e75dcc25f521a..e16f8bed8d36b 100644 --- a/perception/shape_estimation/lib/corrector/utils.cpp +++ b/perception/shape_estimation/lib/corrector/utils.cpp @@ -14,10 +14,11 @@ #include "shape_estimation/corrector/utils.hpp" +#include + #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #include diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 9be64112bc32d..987cf8106c99e 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -15,7 +15,7 @@ #include "shape_estimation/shape_estimator.hpp" #include -#include +#include #include diff --git a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp index 6e2daf9266c9c..0d041439e6092 100644 --- a/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp +++ b/perception/simple_object_merger/include/simple_object_merger/simple_object_merger_node.hpp @@ -16,7 +16,7 @@ #define SIMPLE_OBJECT_MERGER__SIMPLE_OBJECT_MERGER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "autoware_auto_perception_msgs/msg/detected_objects.hpp" diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 506a40e672fb6..73a96da454833 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -14,6 +14,8 @@ #include "simple_object_merger/simple_object_merger_node.hpp" +#include + #include #include #include diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/traffic_light_fine_detector/package.xml index c54912e256c02..8ff9f2d133b52 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/traffic_light_fine_detector/package.xml @@ -4,7 +4,7 @@ traffic_light_fine_detector 0.1.0 The traffic_light_fine_detector package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_map_based_detector/src/node.cpp b/perception/traffic_light_map_based_detector/src/node.cpp index fcf13e3900793..a46be77667bcb 100644 --- a/perception/traffic_light_map_based_detector/src/node.cpp +++ b/perception/traffic_light_map_based_detector/src/node.cpp @@ -17,7 +17,8 @@ #include #include #include -#include +#include +#include #include #include diff --git a/perception/traffic_light_multi_camera_fusion/package.xml b/perception/traffic_light_multi_camera_fusion/package.xml index eb0858152e69e..b08ccfe668085 100644 --- a/perception/traffic_light_multi_camera_fusion/package.xml +++ b/perception/traffic_light_multi_camera_fusion/package.xml @@ -4,7 +4,7 @@ traffic_light_multi_camera_fusion 0.1.0 The traffic_light_multi_camera_fusion package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_occlusion_predictor/package.xml b/perception/traffic_light_occlusion_predictor/package.xml index 0ae390c90b9a9..49a67d68057d9 100644 --- a/perception/traffic_light_occlusion_predictor/package.xml +++ b/perception/traffic_light_occlusion_predictor/package.xml @@ -4,7 +4,7 @@ traffic_light_occlusion_predictor 0.1.0 The traffic_light_occlusion_predictor package - Mingyu Li + Tao Zhong Shunsuke Miura Apache License 2.0 diff --git a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp index 366820a725018..c460f6a623bc4 100644 --- a/perception/traffic_light_occlusion_predictor/src/nodelet.cpp +++ b/perception/traffic_light_occlusion_predictor/src/nodelet.cpp @@ -18,7 +18,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 39d05a7fb761b..e77f7726015af 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -2,7 +2,6 @@ ros__parameters: goal_planner: # general params - minimum_request_length: 100.0 th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. @@ -17,7 +16,7 @@ longitudinal_margin: 3.0 max_lateral_offset: 0.5 lateral_offset_interval: 0.25 - ignore_distance_from_lane_start: 15.0 + ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.5 # occupancy grid map @@ -36,6 +35,7 @@ # pull over pull_over: + minimum_request_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 @@ -104,6 +104,66 @@ neighbor_radius: 8.0 margin: 1.0 + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 + # debug debug: print_debug_info: false diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index bea78664c65a3..b62262423fa72 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -72,3 +72,63 @@ max_planning_time: 150.0 neighbor_radius: 8.0 margin: 1.0 + + stop_condition: + maximum_deceleration_for_stop: 1.0 + maximum_jerk_for_stop: 1.0 + path_safety_check: + # EgoPredictedPath + ego_predicted_path: + acceleration: 1.0 + time_horizon: 10.0 + time_resolution: 0.5 + min_slow_speed: 0.0 + delay_until_departure: 1.0 + target_velocity: 1.0 + # For target object filtering + target_filtering: + safety_check_time_horizon: 5.0 + safety_check_time_resolution: 1.0 + # detection range + object_check_forward_distance: 10.0 + object_check_backward_distance: 100.0 + ignore_object_velocity_threshold: 0.0 + # ObjectTypesToCheck + object_types_to_check: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: false + # ObjectLaneConfiguration + object_lane_configuration: + check_current_lane: true + check_right_side_lane: true + check_left_side_lane: true + check_shoulder_lane: true + check_other_lane: false + include_opposite_lane: false + invert_opposite_lane: false + check_all_predicted_path: true + use_all_predicted_path: true + use_predicted_path_outside_lanelet: false + + # For safety check + safety_check_params: + # safety check configuration + enable_safety_check: true + # collision check parameters + check_all_predicted_path: true + publish_debug_marker: false + rss_params: + rear_vehicle_reaction_time: 1.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 1.0 + longitudinal_velocity_delta_time: 1.0 + # temporary + backward_path_length: 30.0 + forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 0a78787c559d7..4cc8910acf82b 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -86,8 +86,8 @@ Either one is activated when all conditions are met. ### fixed_goal_planner -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. - Route is set with `allow_goal_modification=false` by default. +- ego-vehicle is in the same lane as the goal. @@ -95,7 +95,7 @@ Either one is activated when all conditions are met. #### pull over on road lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Route is set with `allow_goal_modification=true` . - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service. - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz. @@ -105,7 +105,7 @@ Either one is activated when all conditions are met. #### pull over on shoulder lane -- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`. +- The distance between the goal and ego-vehicle is shorter than `pull_over_minimum_request_length`. - Goal is set in the `road_shoulder`. @@ -118,17 +118,11 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated. | 100.0 | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | -| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | -| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | -| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | +| Name | Unit | Type | Description | Default value | +| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | ## **collision check** @@ -174,12 +168,22 @@ searched for in certain range of the shoulder lane. | longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | | max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | | lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 15.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | -## **Path Generation** +## **Pull Over** There are three path generation methods. -The path is generated with a certain margin (default: `0.5 m`) from left boundary of shoulder lane. +The path is generated with a certain margin (default: `0.5 m`) from the boundary of shoulder lane. + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pull_over_minimum_request_length | [m] | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, pull over is activated. | 100.0 | +| pull_over_velocity | [m/s] | double | decelerate to this speed by the goal search area | 3.0 | +| pull_over_minimum_velocity | [m/s] | double | speed of pull_over after stopping once. this prevents excessive acceleration. | 1.38 | +| decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | +| maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | ### **shift parking** diff --git a/planning/behavior_path_planner/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 d7f824ba8f419..ab6f77b1f53d6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -24,6 +24,9 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -57,6 +60,12 @@ using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; + enum class PathType { NONE = 0, SHIFT, @@ -107,6 +116,13 @@ class GoalPlannerModule : public SceneModuleInterface void updateModuleParams(const std::any & parameters) override { parameters_ = std::any_cast>(parameters); + if (parameters_->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } // TODO(someone): remove this, and use base class function @@ -137,8 +153,14 @@ class GoalPlannerModule : public SceneModuleInterface PullOverStatus status_; + mutable StartGoalPlannerData goal_planner_data_; + std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; + vehicle_info_util::VehicleInfo vehicle_info_; // planner @@ -276,6 +298,13 @@ class GoalPlannerModule : public SceneModuleInterface // rtc std::pair calcDistanceToPathChange() const; + // safety check + SafetyCheckParams createSafetyCheckParams() const; + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; + // debug void setDebugData(); void printParkingPositionError() const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 2e8c9e57f8842..ba942da69732d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -17,12 +17,15 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner/utils/utils.hpp" #include #include @@ -43,6 +46,11 @@ namespace behavior_path_planner { +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; +using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; +using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; +using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; +using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; @@ -55,7 +63,8 @@ struct PullOutStatus lanelet::ConstLanelets pull_out_lanes{}; bool is_safe_static_objects{false}; // current path is safe against static objects bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects - bool back_finished{false}; + bool back_finished{false}; // if backward driving is not required, this is also set to true + // todo: rename to clear variable name. Pose pull_out_start_pose{}; PullOutStatus() {} @@ -90,6 +99,13 @@ class StartPlannerModule : public SceneModuleInterface void setParameters(const std::shared_ptr & parameters) { parameters_ = parameters; + if (parameters->safety_check_params.enable_safety_check) { + ego_predicted_path_params_ = + std::make_shared(parameters_->ego_predicted_path_params); + objects_filtering_params_ = + std::make_shared(parameters_->objects_filtering_params); + safety_check_params_ = std::make_shared(parameters_->safety_check_params); + } } void resetStatus(); @@ -110,10 +126,14 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } std::shared_ptr parameters_; + mutable std::shared_ptr ego_predicted_path_params_; + mutable std::shared_ptr objects_filtering_params_; + mutable std::shared_ptr safety_check_params_; vehicle_info_util::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; + mutable StartGoalPlannerData start_planner_data_; std::deque odometry_buffer_; @@ -131,7 +151,8 @@ class StartPlannerModule : public SceneModuleInterface std::mutex mutex_; PathWithLaneId getFullPath() const; - std::vector searchPullOutStartPoses(); + PathWithLaneId calcStartPoseCandidatesBackwardPath() const; + std::vector searchPullOutStartPoses(const PathWithLaneId & start_pose_candidates) const; std::shared_ptr lane_departure_checker_; @@ -141,8 +162,8 @@ class StartPlannerModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; void planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority); + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority); PathWithLaneId generateStopPath() const; lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; @@ -155,6 +176,10 @@ class StartPlannerModule : public SceneModuleInterface bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); + void updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const; + bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. @@ -163,6 +188,7 @@ class StartPlannerModule : public SceneModuleInterface // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); + SafetyCheckParams createSafetyCheckParams() const; // freespace planner void onFreespacePlannerTimer(); bool planFreespacePath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 6b8a62d2caec9..c458d017f518c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -41,7 +41,6 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double minimum_request_length; double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; @@ -72,6 +71,7 @@ struct GoalPlannerParameters double object_recognition_collision_check_max_extra_stopping_margin; // pull over general params + double pull_over_minimum_request_length; double pull_over_velocity; double pull_over_minimum_velocity; double decide_path_distance; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index 60bbb3c78b401..bea671c266899 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -63,7 +63,7 @@ void updateEgoPredictedPathParams( void updateEgoPredictedPathParams( std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateSafetyCheckParams( std::shared_ptr & safety_check_params, @@ -71,7 +71,7 @@ void updateSafetyCheckParams( void updateSafetyCheckParams( std::shared_ptr & safety_check_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, @@ -79,7 +79,7 @@ void updateObjectsFilteringParams( void updateObjectsFilteringParams( std::shared_ptr & objects_filtering_params, - const std::shared_ptr & start_planner_params); + const std::shared_ptr & goal_planner_params); void updatePathProperty( std::shared_ptr & ego_predicted_path_params, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index abdf17c9c6cfe..2b2de183b2dac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -41,7 +41,7 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() override { return PlannerType::FREESPACE; } - boost::optional plan(Pose start_pose, Pose end_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index 7ace770dc7ff1..a5ff52e74038f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 6e6e5111e5dd9..cb662efd767bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -60,7 +60,7 @@ class PullOutPlannerBase } virtual PlannerType getPlannerType() = 0; - virtual boost::optional plan(Pose start_pose, Pose goal_pose) = 0; + virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 2042593064c51..0842d0a17bd97 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(Pose start_pose, Pose goal_pose) override; + boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 63e374e074a5a..d85e6181764d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -16,6 +16,9 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -36,6 +39,7 @@ namespace behavior_path_planner::start_planner_utils using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index be227f4f7e93e..0caa79ba5d062 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -323,8 +323,8 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side); // misc 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 9e7b101b8c4ab..6f22ee9b1412b 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" @@ -275,7 +276,7 @@ bool GoalPlannerModule::isExecutionRequested() const const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; @@ -306,12 +307,19 @@ bool GoalPlannerModule::isExecutionRequested() const return false; } + // if goal modification is not allowed + // 1) goal_pose is in current_lanes, plan path to the original fixed goal + // 2) goal_pose is NOT in current_lanes, do not execute goal_planner + if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + return goal_is_in_current_lanes; + } + // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() - : parameters_->minimum_request_length; + : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -340,6 +348,22 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { + // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. + if (status_.pull_over_path == nullptr) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -348,17 +372,21 @@ double GoalPlannerModule::calcModuleRequestLength() const const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return parameters_->minimum_request_length; + return parameters_->pull_over_minimum_request_length; } const double minimum_request_length = *min_stop_distance + parameters_->backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->minimum_request_length); + return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const { + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(*(planner_data_->route_handler), left_side_parking_); @@ -390,15 +418,18 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const center_pose.orientation = tf2::toMsg(tf_quat); } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) { + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) { RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); return goal_pose; } + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - distance_from_left_bound.value() + parameters_->margin_from_boundary; + sign * (distance_from_bound.value() + parameters_->margin_from_boundary); + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; @@ -504,7 +535,7 @@ void GoalPlannerModule::generateGoalCandidates() } // calculate goal candidates - const Pose goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); refined_goal_pose_ = calcRefinedGoal(goal_pose); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); @@ -1121,7 +1152,7 @@ bool GoalPlannerModule::isStuck() bool GoalPlannerModule::hasFinishedCurrentPath() { - const auto & current_path_end = getCurrentPath().points.back(); + const auto current_path_end = getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; const bool is_near_target = tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < parameters_->th_arrived_distance; @@ -1434,12 +1465,82 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } +void GoalPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + goal_planner_data_.filtered_objects = filtered_objects; + goal_planner_data_.target_objects_on_lane = target_objects_on_lane; + goal_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool GoalPlannerModule::isSafePath() const +{ + const auto pull_over_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = planner_data_->parameters.backward_path_length; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const auto pull_over_lanes = + goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + RCLCPP_DEBUG( + getLogger(), "pairs_terminal_velocity_and_accel: %f, %f", terminal_velocity_and_accel.first, + terminal_velocity_and_accel.second); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, pull_over_lanes, current_pose.position, + objects_filtering_params_); + + const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_over_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + void GoalPlannerModule::setDebugData() { debug_marker_.markers.clear(); + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1481,6 +1582,17 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + if (goal_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (goal_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } } // Visualize planner type text @@ -1541,7 +1653,7 @@ void GoalPlannerModule::printParkingPositionError() const bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const { const auto & route_handler = planner_data_->route_handler; - const Pose & goal_pose = route_handler->getGoalPose(); + const Pose & goal_pose = route_handler->getOriginalGoalPose(); const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes(*(route_handler), left_side_parking_); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 209b12c42537c..7e2975a73aa44 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -32,18 +32,17 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { GoalPlannerParameters p; + const std::string base_ns = "goal_planner."; // general params { - std::string ns = "goal_planner."; - p.minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); - p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); - p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); - p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); + p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); + p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); + p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); } // goal search { - std::string ns = "goal_planner.goal_search."; + const std::string ns = base_ns + "goal_search."; p.search_priority = node->declare_parameter(ns + "search_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); @@ -72,7 +71,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { - std::string ns = "goal_planner.occupancy_grid."; + const std::string ns = base_ns + "occupancy_grid."; p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); p.use_occupancy_grid_for_longitudinal_margin = node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); @@ -84,7 +83,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // object recognition { - std::string ns = "goal_planner.object_recognition."; + const std::string ns = base_ns + "object_recognition."; p.use_object_recognition = node->declare_parameter(ns + "use_object_recognition"); p.object_recognition_collision_check_margin = node->declare_parameter(ns + "object_recognition_collision_check_margin"); @@ -95,7 +94,9 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // pull over general params { - std::string ns = "goal_planner.pull_over."; + const std::string ns = base_ns + "pull_over."; + p.pull_over_minimum_request_length = + node->declare_parameter(ns + "minimum_request_length"); p.pull_over_velocity = node->declare_parameter(ns + "pull_over_velocity"); p.pull_over_minimum_velocity = node->declare_parameter(ns + "pull_over_minimum_velocity"); @@ -106,7 +107,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // shift parking { - std::string ns = "goal_planner.pull_over.shift_parking."; + const std::string ns = base_ns + "pull_over.shift_parking."; p.enable_shift_parking = node->declare_parameter(ns + "enable_shift_parking"); p.shift_sampling_num = node->declare_parameter(ns + "shift_sampling_num"); p.maximum_lateral_jerk = node->declare_parameter(ns + "maximum_lateral_jerk"); @@ -118,7 +119,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking forward { - std::string ns = "goal_planner.pull_over.parallel_parking.forward."; + const std::string ns = base_ns + "pull_over.parallel_parking.forward."; p.enable_arc_forward_parking = node->declare_parameter(ns + "enable_arc_forward_parking"); p.parallel_parking_parameters.after_forward_parking_straight_distance = node->declare_parameter(ns + "after_forward_parking_straight_distance"); @@ -134,7 +135,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // forward parallel parking backward { - std::string ns = "goal_planner.pull_over.parallel_parking.backward."; + const std::string ns = base_ns + "pull_over.parallel_parking.backward."; p.enable_arc_backward_parking = node->declare_parameter(ns + "enable_arc_backward_parking"); p.parallel_parking_parameters.after_backward_parking_straight_distance = @@ -151,7 +152,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking general params { - std::string ns = "goal_planner.pull_over.freespace_parking."; + const std::string ns = base_ns + "pull_over.freespace_parking."; p.enable_freespace_parking = node->declare_parameter(ns + "enable_freespace_parking"); p.freespace_parking_algorithm = node->declare_parameter(ns + "freespace_parking_algorithm"); @@ -174,7 +175,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking search config { - std::string ns = "goal_planner.pull_over.freespace_parking.search_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.search_configs."; p.freespace_parking_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_parking_common_parameters.angle_goal_range = @@ -191,14 +192,14 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking costmap configs { - std::string ns = "goal_planner.pull_over.freespace_parking.costmap_configs."; + const std::string ns = base_ns + "pull_over.freespace_parking.costmap_configs."; p.freespace_parking_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace parking astar { - std::string ns = "goal_planner.pull_over.freespace_parking.astar."; + const std::string ns = base_ns + "pull_over.freespace_parking.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -208,7 +209,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // freespace parking rrtstar { - std::string ns = "goal_planner.pull_over.freespace_parking.rrtstar."; + const std::string ns = base_ns + "pull_over.freespace_parking.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -218,9 +219,125 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string path_safety_check_ns = "goal_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // debug { - std::string ns = "goal_planner.debug."; + const std::string ns = base_ns + "debug."; p.print_debug_info = node->declare_parameter(ns + "print_debug_info"); } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 9d2da0d13f56b..5f4b57d9c6367 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -148,6 +148,122 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // stop condition + { + p.maximum_deceleration_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); + } + + std::string base_ns = "start_planner.path_safety_check."; + + // EgoPredictedPath + std::string ego_path_ns = base_ns + "ego_predicted_path."; + { + p.ego_predicted_path_params.acceleration = + node->declare_parameter(ego_path_ns + "acceleration"); + p.ego_predicted_path_params.time_horizon = + node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.time_resolution = + node->declare_parameter(ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.min_slow_speed = + node->declare_parameter(ego_path_ns + "min_slow_speed"); + p.ego_predicted_path_params.delay_until_departure = + node->declare_parameter(ego_path_ns + "delay_until_departure"); + p.ego_predicted_path_params.target_velocity = + node->declare_parameter(ego_path_ns + "target_velocity"); + } + + // ObjectFilteringParams + std::string obj_filter_ns = base_ns + "target_filtering."; + { + p.objects_filtering_params.safety_check_time_horizon = + node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + node->declare_parameter(obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + node->declare_parameter(obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + node->declare_parameter(obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + node->declare_parameter(obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + node->declare_parameter(obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + node->declare_parameter(obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + node->declare_parameter(obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + node->declare_parameter(obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + node->declare_parameter(obj_filter_ns + "use_predicted_path_outside_lanelet"); + } + + // ObjectTypesToCheck + std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + { + p.objects_filtering_params.object_types_to_check.check_car = + node->declare_parameter(obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + node->declare_parameter(obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + node->declare_parameter(obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + node->declare_parameter(obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + node->declare_parameter(obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + node->declare_parameter(obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + node->declare_parameter(obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + node->declare_parameter(obj_types_ns + "check_pedestrian"); + } + + // ObjectLaneConfiguration + std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + { + p.objects_filtering_params.object_lane_configuration.check_current_lane = + node->declare_parameter(obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + node->declare_parameter(obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + node->declare_parameter(obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + node->declare_parameter(obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + node->declare_parameter(obj_lane_ns + "check_other_lane"); + } + + // SafetyCheckParams + std::string safety_check_ns = base_ns + "safety_check_params."; + { + p.safety_check_params.enable_safety_check = + node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.backward_path_length = + node->declare_parameter(safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + node->declare_parameter(safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + node->declare_parameter(safety_check_ns + "publish_debug_marker"); + } + + // RSSparams + std::string rss_ns = safety_check_ns + "rss_params."; + { + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + node->declare_parameter(rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + node->declare_parameter(rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + node->declare_parameter(rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index abdd41f4935c5..90078a50e6ac9 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -17,7 +17,9 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include #include @@ -108,7 +110,7 @@ bool StartPlannerModule::isExecutionRequested() const { // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. // Execute when current pose is near route start pose - const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > @@ -117,7 +119,7 @@ bool StartPlannerModule::isExecutionRequested() const } // Check if ego arrives at goal - const Pose & goal_pose = planner_data_->route_handler->getGoalPose(); + const Pose goal_pose = planner_data_->route_handler->getGoalPose(); if ( tier4_autoware_utils::calcDistance2d(goal_pose.position, current_pose.position) < parameters_->th_arrived_distance) { @@ -134,41 +136,26 @@ bool StartPlannerModule::isExecutionRequested() const return false; } - // Create vehicle footprint - const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - const auto vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(planner_data_->self_odometry->pose.pose)); - - // Check if ego is not out of lanes - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); - auto lanes = current_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } - - if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { - return false; - } - return true; } bool StartPlannerModule::isExecutionReady() const { + if (status_.pull_out_path.partial_paths.empty()) { + return true; + } + + if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + utils::start_goal_planner_common::updateEgoPredictedPathParams( + ego_predicted_path_params_, parameters_); + utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_); + utils::start_goal_planner_common::updateObjectsFilteringParams( + objects_filtering_params_, parameters_); + if (!isSafePath()) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + return false; + } + } return true; } @@ -407,8 +394,8 @@ PathWithLaneId StartPlannerModule::getCurrentPath() const } void StartPlannerModule::planWithPriority( - const std::vector & start_pose_candidates, const Pose & goal_pose, - const std::string search_priority) + const std::vector & start_pose_candidates, const Pose & refined_start_pose, + const Pose & goal_pose, const std::string search_priority) { // check if start pose candidates are valid if (start_pose_candidates.empty()) { @@ -416,12 +403,13 @@ void StartPlannerModule::planWithPriority( } const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Set back_finished flag based on the current index - status_.back_finished = i == 0; - // Get the pull_out_start_pose for the current index const auto & pull_out_start_pose = start_pose_candidates.at(i); + // Set back_finished to true if the current start pose is same to refined_start_pose + status_.back_finished = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + planner->setPlannerData(planner_data_); const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); // not found safe path @@ -497,7 +485,6 @@ void StartPlannerModule::planWithPriority( for (const auto & p : order_priority) { if (is_safe_with_pose_planner(p.first, p.second)) { - const std::lock_guard lock(mutex_); return; } } @@ -615,9 +602,24 @@ void StartPlannerModule::updatePullOutStatus() const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & goal_pose = planner_data_->route_handler->getGoalPose(); + // refine start pose with pull out lanes. + // 1) backward driving is not allowed: use refined pose just as start pose. + // 2) backward driving is allowed: use refined pose to check if backward driving is needed. + const PathWithLaneId start_pose_candidates_path = calcStartPoseCandidatesBackwardPath(); + const auto refined_start_pose = calcLongitudinalOffsetPose( + start_pose_candidates_path.points, planner_data_->self_odometry->pose.pose.position, 0.0); + if (!refined_start_pose) return; + // search pull out start candidates backward - std::vector start_pose_candidates = searchPullOutStartPoses(); - planWithPriority(start_pose_candidates, goal_pose, parameters_->search_priority); + const std::vector start_pose_candidates = std::invoke([&]() -> std::vector { + if (parameters_->enable_back) { + return searchPullOutStartPoses(start_pose_candidates_path); + } + return {*refined_start_pose}; + }); + + planWithPriority( + start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); checkBackFinished(); if (!status_.back_finished) { @@ -627,20 +629,34 @@ void StartPlannerModule::updatePullOutStatus() } } -std::vector StartPlannerModule::searchPullOutStartPoses() +PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { - std::vector pull_out_start_pose{}; - const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path const auto arc_position_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, current_pose); const double check_distance = parameters_->max_back_distance + 30.0; // buffer - auto backward_shoulder_path = planner_data_->route_handler->getCenterLinePath( + auto path = planner_data_->route_handler->getCenterLinePath( status_.pull_out_lanes, arc_position_pose.length - check_distance, arc_position_pose.length + check_distance); + // lateral shift to current_pose + const double distance_from_center_line = arc_position_pose.distance; + for (auto & p : path.points) { + p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); + } + + return path; +} + +std::vector StartPlannerModule::searchPullOutStartPoses( + const PathWithLaneId & start_pose_candidates) const +{ + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + + std::vector pull_out_start_pose{}; + // filter pull out lanes stop objects const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -648,28 +664,12 @@ std::vector StartPlannerModule::searchPullOutStartPoses() const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); - // lateral shift to current_pose - const double distance_from_center_line = arc_position_pose.distance; - for (auto & p : backward_shoulder_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); - } - - // if backward driving is disable, just refine current pose to the lanes - if (!parameters_->enable_back) { - const auto refined_pose = - calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); - if (refined_pose) { - pull_out_start_pose.push_back(*refined_pose); - } - return pull_out_start_pose; - } - // 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; back_distance += parameters_->backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( - backward_shoulder_path.points, current_pose.position, -back_distance); + start_pose_candidates.points, current_pose.position, -back_distance); if (!backed_pose) { continue; } @@ -677,10 +677,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses() // check the back pose is near the lane end const double length_to_backed_pose = lanelet::utils::getArcCoordinates(status_.pull_out_lanes, *backed_pose).length; - double length_to_lane_end = 0.0; - for (const auto & lane : status_.pull_out_lanes) { - length_to_lane_end += lanelet::utils::getLaneletLength2d(lane); - } + + const double length_to_lane_end = std::accumulate( + std::begin(status_.pull_out_lanes), std::end(status_.pull_out_lanes), 0.0, + [](double acc, const auto & lane) { return acc + lanelet::utils::getLaneletLength2d(lane); }); const double distance_from_lane_end = length_to_lane_end - length_to_backed_pose; if (distance_from_lane_end < parameters_->ignore_distance_from_lane_end) { RCLCPP_WARN_THROTTLE( @@ -923,6 +923,70 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return turn_signal; } +void StartPlannerModule::updateSafetyCheckTargetObjectsData( + const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, + const std::vector & ego_predicted_path) const +{ + start_planner_data_.filtered_objects = filtered_objects; + start_planner_data_.target_objects_on_lane = target_objects_on_lane; + start_planner_data_.ego_predicted_path = ego_predicted_path; +} + +bool StartPlannerModule::isSafePath() const +{ + // TODO(Sugahara): should safety check for backward path + + const auto pull_out_path = getCurrentPath(); + const auto & current_pose = planner_data_->self_odometry->pose.pose; + const double current_velocity = std::hypot( + planner_data_->self_odometry->twist.twist.linear.x, + planner_data_->self_odometry->twist.twist.linear.y); + const auto & dynamic_object = planner_data_->dynamic_object; + const auto & route_handler = planner_data_->route_handler; + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_out_path.points); + const auto & common_param = planner_data_->parameters; + const std::pair terminal_velocity_and_accel = + utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( + status_.pull_out_path.pairs_terminal_velocity_and_accel, status_.current_path_idx); + utils::start_goal_planner_common::updatePathProperty( + ego_predicted_path_params_, terminal_velocity_and_accel); + const auto ego_predicted_path = + behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, + ego_seg_idx); + + const auto filtered_objects = utils::path_safety_checker::filterObjects( + dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); + + const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + current_lanes, route_handler, filtered_objects, objects_filtering_params_); + + const double hysteresis_factor = + status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + updateSafetyCheckTargetObjectsData(filtered_objects, target_objects_on_lane, ego_predicted_path); + + for (const auto & object : target_objects_on_lane.on_current_lane) { + const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( + object, objects_filtering_params_->check_all_predicted_path); + for (const auto & obj_path : obj_predicted_paths) { + CollisionCheckDebug collision{}; + if (!utils::path_safety_checker::checkCollision( + pull_out_path, ego_predicted_path, object, obj_path, common_param, + safety_check_params_->rss_params, hysteresis_factor, collision)) { + return false; + } + } + } + + return true; +} + bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -1041,8 +1105,10 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons void StartPlannerModule::setDebugData() const { + using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using marker_utils::createPredictedPathMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; @@ -1060,6 +1126,17 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } // Visualize planner type text const auto header = planner_data_->route_handler->getRouteHeader(); diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 27dbfb532c1fc..99d109dd0bece 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -447,7 +447,6 @@ std::vector GeometricParallelParking::planOneTrial( if (std::abs(end_pose_offset) > 0) { PathPointWithLaneId straight_point{}; straight_point.point.pose = goal_pose; - // setLaneIds(straight_point); path_turn_right.points.push_back(straight_point); } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 7760b9b57d8ab..5594f7af1c428 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -47,13 +47,12 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) const auto road_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); - const auto shoulder_lanes = + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); - if (road_lanes.empty() || shoulder_lanes.empty()) { + if (road_lanes.empty() || pull_over_lanes.empty()) { return {}; } - auto lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); + const auto lanes = utils::combineLanelets(road_lanes, pull_over_lanes); const auto & p = parallel_parking_parameters_; const double max_steer_angle = @@ -62,7 +61,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, shoulder_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e57dcc319dc5..e12ee576a5d4b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -53,6 +53,9 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) const double lateral_offset_interval = parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(*route_handler, left_side_parking_); @@ -76,7 +79,10 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { - const Pose & center_pose = p.point.pose; + // todo(kosuke55): fix orientation for inverseTransformPoint temporarily + Pose center_pose = p.point.pose; + center_pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(center_pose.orientation)); // ignore goal_pose near lane start const double distance_from_lane_start = @@ -85,13 +91,14 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) continue; } - const auto distance_from_left_bound = utils::getSignedDistanceFromBoundary( - pull_over_lanes, vehicle_footprint_, center_pose, left_side_parking_); - if (!distance_from_left_bound) continue; + const auto distance_from_bound = utils::getSignedDistanceFromBoundary( + pull_over_lanes, vehicle_width, base_link2front, base_link2rear, center_pose, + left_side_parking_); + if (!distance_from_bound) continue; - const double sign = left_side_parking_ ? 1.0 : -1.0; + const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - sign * (std::abs(distance_from_left_bound.value()) - margin_from_boundary); + -distance_from_bound.value() + sign * margin_from_boundary; const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( @@ -102,7 +109,7 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) double lateral_offset = 0.0; for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_search_pose, 0, -sign * dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, sign * dy, 0); const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index 8b48d1a867126..0d58a54431be0 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -57,7 +57,7 @@ PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWith lanelet::ConstLanelets getPullOverLanes(const RouteHandler & route_handler, const bool left_side) { - const Pose goal_pose = route_handler.getGoalPose(); + const Pose goal_pose = route_handler.getOriginalGoalPose(); lanelet::ConstLanelet target_shoulder_lane{}; if (route_handler::RouteHandler::getPullOverTarget( diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index f29711c82b3ce..cff6e9f81f02e 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -131,8 +131,15 @@ void updatePathProperty( std::shared_ptr & ego_predicted_path_params, const std::pair & pairs_terminal_velocity_and_accel) { + // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is + // necessary to ensure a reasonable path length. + // TODO(Sugahara): define them as parameter + const double min_accel_for_ego_predicted_path = 1.0; + const double acceleration = + std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); + ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; - ego_predicted_path_params->acceleration = pairs_terminal_velocity_and_accel.second; + ego_predicted_path_params->acceleration = acceleration; } std::pair getPairsTerminalVelocityAndAccel( diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index ba34901d9df6a..36fb6381ac967 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose start_pose, const Pose end_pose) +boost::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) { const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length; @@ -86,7 +86,7 @@ boost::optional FreespacePullOut::plan(const Pose start_pose, const } // push back generate road lane path between end pose and goal pose to last path - const auto & goal_pose = route_handler->getGoalPose(); + const Pose goal_pose = route_handler->getGoalPose(); constexpr double offset_from_end_pose = 1.0; const auto arc_position_end = lanelet::utils::getArcCoordinates(road_lanes, end_pose); const double s_start = std::max(arc_position_end.length + offset_from_end_pose, 0.0); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 29abdb08c5c55..628f1cfd7e421 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -36,7 +36,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; @@ -47,16 +47,6 @@ boost::optional GeometricPullOut::plan(Pose start_pose, Pose goal_p planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); - auto lanes = road_lanes; - for (const auto & pull_out_lane : pull_out_lanes) { - auto it = std::find_if( - lanes.begin(), lanes.end(), [&pull_out_lane](const lanelet::ConstLanelet & lane) { - return lane.id() == pull_out_lane.id(); - }); - if (it == lanes.end()) { - lanes.push_back(pull_out_lane); - } - } planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 7352821e140be..f76aca793d571 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -40,7 +40,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) +boost::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index c7fde9dada4ec..da23d818cf59a 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2405,64 +2405,126 @@ double getSignedDistanceFromBoundary( } std::optional getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & lanelets, const LinearRing2d & footprint, - const Pose & vehicle_pose, const bool left_side) -{ - double min_distance = std::numeric_limits::max(); - - const auto transformed_footprint = - transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); - bool found_neighbor_bound = false; - for (const auto & vehicle_corner_point : transformed_footprint) { - // convert point of footprint to pose + const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, + const double base_link2rear, const Pose & vehicle_pose, const bool left_side) +{ + // Depending on which side is selected, calculate the transformed coordinates of the front and + // rear vehicle corners + Point rear_corner_point, front_corner_point; + if (left_side) { + Point front_left, rear_left; + rear_left.x = -base_link2rear; + rear_left.y = vehicle_width / 2; + front_left.x = base_link2front; + front_left.y = vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_left, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_left, vehicle_pose); + } else { + Point front_right, rear_right; + rear_right.x = -base_link2rear; + rear_right.y = -vehicle_width / 2; + front_right.x = base_link2front; + front_right.y = -vehicle_width / 2; + rear_corner_point = tier4_autoware_utils::transformPoint(rear_right, vehicle_pose); + front_corner_point = tier4_autoware_utils::transformPoint(front_right, vehicle_pose); + } + + const auto combined_lane = lanelet::utils::combineLaneletsShape(lanelets); + const auto & bound_line_2d = left_side ? lanelet::utils::to2D(combined_lane.leftBound3d()) + : lanelet::utils::to2D(combined_lane.rightBound3d()); + + // Initialize the lateral distance to the maximum (for left side) or the minimum (for right side) + // possible value. + double lateral_distance = + left_side ? -std::numeric_limits::max() : std::numeric_limits::max(); + + // Find the closest bound segment that contains the corner point in the X-direction + // and calculate the lateral distance from that segment. + const auto calcLateralDistanceFromBound = [&]( + const Point & vehicle_corner_point, + boost::optional & lateral_distance, + boost::optional & segment_idx) { Pose vehicle_corner_pose{}; - vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.position = vehicle_corner_point; vehicle_corner_pose.orientation = vehicle_pose.orientation; - // calculate distance to the bound directly next to footprint points - lanelet::ConstLanelet closest_lanelet{}; - if (lanelet::utils::query::getClosestLanelet(lanelets, vehicle_corner_pose, &closest_lanelet)) { - const auto & bound_line_2d = left_side ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - - for (size_t i = 1; i < bound_line_2d.size(); ++i) { - const Point p_front = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i - 1]); - const Point p_back = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); - - const Point inverse_p_front = - tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); - const Point inverse_p_back = - tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); - - const double dy_front = inverse_p_front.y; - const double dy_back = inverse_p_back.y; - const double dx_front = inverse_p_front.x; - const double dx_back = inverse_p_back.x; - // is in segment - if (dx_front < 0 && dx_back > 0) { - const double lateral_distance_from_pose_to_segment = - (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); - - if (std::abs(lateral_distance_from_pose_to_segment) < std::abs(min_distance)) { - min_distance = lateral_distance_from_pose_to_segment; - } - - found_neighbor_bound = true; - break; - } + // Euclidean distance to find the closest segment containing the corner point. + double min_distance = std::numeric_limits::max(); + + for (size_t i = 0; i < bound_line_2d.size() - 1; i++) { + const Point p1 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + const Point p2 = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i + 1]); + + const Point inverse_p1 = tier4_autoware_utils::inverseTransformPoint(p1, vehicle_corner_pose); + const Point inverse_p2 = tier4_autoware_utils::inverseTransformPoint(p2, vehicle_corner_pose); + const double dx_p1 = inverse_p1.x; + const double dx_p2 = inverse_p2.x; + const double dy_p1 = inverse_p1.y; + const double dy_p2 = inverse_p2.y; + + // Calculate the Euclidean distances between vehicle's corner and the current and next points. + const double distance1 = tier4_autoware_utils::calcDistance2d(p1, vehicle_corner_point); + const double distance2 = tier4_autoware_utils::calcDistance2d(p2, vehicle_corner_point); + + // If one of the bound points is behind and the other is in front of the vehicle corner point + // and any of these points is closer than the current minimum distance, + // then update minimum distance, lateral distance and the segment index. + if (dx_p1 < 0 && dx_p2 > 0 && (distance1 < min_distance || distance2 < min_distance)) { + min_distance = std::min(distance1, distance2); + // Update lateral distance using the formula derived from similar triangles in the lateral + // cross-section view. + lateral_distance = -1.0 * (dy_p1 * dx_p2 + dy_p2 * -dx_p1) / (dx_p2 - dx_p1); + segment_idx = i; } } - } + }; - if (!found_neighbor_bound) { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "neighbor shoulder bound to footprint is not found."); + // Calculate the lateral distance for both the rear and front corners of the vehicle. + boost::optional rear_segment_idx{}; + boost::optional rear_lateral_distance{}; + calcLateralDistanceFromBound(rear_corner_point, rear_lateral_distance, rear_segment_idx); + boost::optional front_segment_idx{}; + boost::optional front_lateral_distance{}; + calcLateralDistanceFromBound(front_corner_point, front_lateral_distance, front_segment_idx); + + // If no closest bound segment was found for both corners, return an empty optional. + if (!rear_lateral_distance && !front_lateral_distance) { return {}; } - - // min_distance is the distance from corner_pose to bound, so reverse this value - return -min_distance; + // If only one of them found the closest bound, return the found lateral distance. + if (!rear_lateral_distance) { + return *front_lateral_distance; + } else if (!front_lateral_distance) { + return *rear_lateral_distance; + } + // If both corners found their closest bound, return the maximum (for left side) or the minimum + // (for right side) lateral distance. + lateral_distance = left_side ? std::max(*rear_lateral_distance, *front_lateral_distance) + : std::min(*rear_lateral_distance, *front_lateral_distance); + + // Iterate through all segments between the segments closest to the rear and front corners. + // Update the lateral distance in case any of these inner segments are closer to the vehicle. + for (size_t i = *rear_segment_idx + 1; i < *front_segment_idx; i++) { + Pose bound_pose; + bound_pose.position = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); + bound_pose.orientation = vehicle_pose.orientation; + + const Point inverse_rear_point = + tier4_autoware_utils::inverseTransformPoint(rear_corner_point, bound_pose); + const Point inverse_front_point = + tier4_autoware_utils::inverseTransformPoint(front_corner_point, bound_pose); + const double dx_rear = inverse_rear_point.x; + const double dx_front = inverse_front_point.x; + const double dy_rear = inverse_rear_point.y; + const double dy_front = inverse_front_point.y; + + const double current_lateral_distance = + (dy_rear * dx_front + dy_front * -dx_rear) / (dx_front - dx_rear); + lateral_distance = left_side ? std::max(lateral_distance, current_lateral_distance) + : std::min(lateral_distance, current_lateral_distance); + } + + return lateral_distance; } double getArcLengthToTargetLanelet( diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index feb6e42587196..6d2a3110cbdb7 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -37,6 +37,7 @@ using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; +using motion_utils::calcSignedArcLengthPartialSum; using motion_utils::findNearestSegmentIndex; using motion_utils::insertTargetPoint; using motion_utils::resamplePath; @@ -751,15 +752,15 @@ Polygon2d CrosswalkModule::getAttentionArea( { const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_polygon = createVehiclePolygon(planner_data_->vehicle_info_); + const auto backward_path_length = + calcSignedArcLength(sparse_resample_path.points, size_t(0), ego_pos); + const auto length_sum = calcSignedArcLengthPartialSum( + sparse_resample_path.points, size_t(0), sparse_resample_path.points.size()); Polygon2d attention_area; for (size_t j = 0; j < sparse_resample_path.points.size() - 1; ++j) { - const auto & p_ego_front = sparse_resample_path.points.at(j).point.pose; - const auto & p_ego_back = sparse_resample_path.points.at(j + 1).point.pose; - const auto front_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_front.position); - const auto back_length = - calcSignedArcLength(sparse_resample_path.points, ego_pos, p_ego_back.position); + const auto front_length = length_sum.at(j) - backward_path_length; + const auto back_length = length_sum.at(j + 1) - backward_path_length; if (back_length < crosswalk_attention_range.first) { continue; diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index da46055460a99..885c0dc2859ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -140,9 +140,9 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | Parameter /objects | Type | Description | | ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] consider objects with an estimated time to collision bellow this value while on the overlap | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | +| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | | `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | +| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | | Parameter /overlap | Type | Description | | ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | @@ -155,7 +155,7 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | `strict` | bool | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane | | `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | | `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | +| `slowdown.velocity` | double | [m/s] slow down velocity | | `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 1818c460e407b..a797edaa34556 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -98,6 +98,7 @@ class RouteHandler Pose getGoalPose() const; Pose getStartPose() const; Pose getOriginalStartPose() const; + Pose getOriginalGoalPose() const; lanelet::Id getGoalLaneId() const; bool getGoalLanelet(lanelet::ConstLanelet * goal_lanelet) const; std::vector getLanesBeforePose( @@ -378,6 +379,7 @@ class RouteHandler // save original(not modified) route start pose for start planer execution Pose original_start_pose_; + Pose original_goal_pose_; // non-const methods void setLaneletsFromRouteMsg(); diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index b8cedaa947455..91f86ed0e454f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -176,6 +176,7 @@ void RouteHandler::setRoute(const LaneletRoute & route_msg) // if get not modified route but new route, reset original start pose if (!route_ptr_ || route_ptr_->uuid != route_msg.uuid) { original_start_pose_ = route_msg.start_pose; + original_goal_pose_ = route_msg.goal_pose; } route_ptr_ = std::make_shared(route_msg); is_handler_ready_ = false; @@ -426,13 +427,17 @@ Pose RouteHandler::getStartPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getStartPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->start_pose; } Pose RouteHandler::getOriginalStartPose() const { + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalStartPose: Route has not been set yet"); + return Pose(); + } return original_start_pose_; } @@ -440,11 +445,20 @@ Pose RouteHandler::getGoalPose() const { if (!route_ptr_) { RCLCPP_WARN(logger_, "[Route Handler] getGoalPose: Route has not been set yet"); - Pose(); + return Pose(); } return route_ptr_->goal_pose; } +Pose RouteHandler::getOriginalGoalPose() const +{ + if (!route_ptr_) { + RCLCPP_WARN(logger_, "[Route Handler] getOriginalGoalPose: Route has not been set yet"); + return Pose(); + } + return original_goal_pose_; +} + lanelet::Id RouteHandler::getGoalLaneId() const { if (!route_ptr_ || route_ptr_->segments.empty()) {