diff --git a/.github/labeler.yaml b/.github/labeler.yaml index c3efa2a1a2b15..47f8737ebbf39 100644 --- a/.github/labeler.yaml +++ b/.github/labeler.yaml @@ -31,7 +31,7 @@ - planning/**/* "component:sensing": - sensing/**/* -"component:simulator": +"component:simulation": - simulator/**/* "component:system": - system/**/* diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 4d316b9fb2481..5b60a371abd93 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -12,8 +12,6 @@ - source: .github/PULL_REQUEST_TEMPLATE/standard-change.md - source: .github/dependabot.yaml - source: .github/stale.yml - pre-commands: | - sd "staleLabel: stale" "staleLabel: status:stale" {source} - source: .github/workflows/cancel-previous-workflows.yaml - source: .github/workflows/github-release.yaml - source: .github/workflows/pre-commit.yaml diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index c77503106e485..dc55576640133 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -46,7 +47,9 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::LaneletRoute; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::PoseDeviation; +using tier4_autoware_utils::Segment2d; using TrajectoryPoints = std::vector; +typedef boost::geometry::index::rtree> SegmentRtree; struct Param { @@ -137,13 +140,15 @@ class LaneDepartureChecker const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints); + double calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const; + + static SegmentRtree extractUncrossableBoundaries( + const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, + const double max_search_length, const std::vector & boundary_types_to_detect); + static bool willCrossBoundary( - const lanelet::ConstLanelets & candidate_lanelets, const std::vector & vehicle_footprints, - const std::vector & boundary_types_to_detects); - - static bool isCrossingRoadBorder( - const lanelet::BasicLineString2d & road_border, const std::vector & footprints); + const SegmentRtree & uncrossable_segments); }; } // namespace lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 9d413b2bf0a70..5bd0fcace9909 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -30,7 +30,9 @@ #include #include +using motion_utils::calcArcLength; using tier4_autoware_utils::LinearRing2d; +using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiPoint2d; using tier4_autoware_utils::Point2d; @@ -41,15 +43,6 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Point; -Point fromVector2dToMsg(const Eigen::Vector2d & point) -{ - Point msg{}; - msg.x = point.x(); - msg.y = point.y(); - msg.z = 0.0; - return msg; -} - double calcBrakingDistance( const double abs_velocity, const double max_deceleration, const double delay_time) { @@ -67,27 +60,6 @@ bool isInAnyLane(const lanelet::ConstLanelets & candidate_lanelets, const Point2 return false; } -bool isCrossingWithBoundary( - const lanelet::BasicLineString2d & boundary, const std::vector & footprints) -{ - for (auto & footprint : footprints) { - for (size_t i = 0; i < footprint.size() - 1; ++i) { - auto footprint1 = footprint.at(i).to_3d(); - auto footprint2 = footprint.at(i + 1).to_3d(); - for (size_t i = 0; i < boundary.size() - 1; ++i) { - auto boundary1 = boundary.at(i); - auto boundary2 = boundary.at(i + 1); - if (tier4_autoware_utils::intersect( - tier4_autoware_utils::toMsg(footprint1), tier4_autoware_utils::toMsg(footprint2), - fromVector2dToMsg(boundary1), fromVector2dToMsg(boundary2))) { - return true; - } - } - } - } - return false; -} - LinearRing2d createHullFromFootprints(const std::vector & footprints) { MultiPoint2d combined; @@ -172,8 +144,12 @@ Output LaneDepartureChecker::update(const Input & input) output.is_out_of_lane = isOutOfLane(output.candidate_lanelets, output.vehicle_footprints.front()); output.processing_time_map["isOutOfLane"] = stop_watch.toc(true); - output.will_cross_boundary = willCrossBoundary( - output.candidate_lanelets, output.vehicle_footprints, input.boundary_types_to_detect); + const double max_search_length_for_boundaries = + calcMaxSearchLengthForBoundaries(*input.predicted_trajectory); + const auto uncrossable_boundaries = extractUncrossableBoundaries( + *input.lanelet_map, input.predicted_trajectory->points.front().pose.position, + max_search_length_for_boundaries, input.boundary_types_to_detect); + output.will_cross_boundary = willCrossBoundary(output.vehicle_footprints, uncrossable_boundaries); output.processing_time_map["willCrossBoundary"] = stop_watch.toc(true); return output; @@ -334,38 +310,58 @@ bool LaneDepartureChecker::isOutOfLane( return false; } -bool LaneDepartureChecker::willCrossBoundary( - const lanelet::ConstLanelets & candidate_lanelets, - const std::vector & vehicle_footprints, - const std::vector & boundary_types_to_detect) +double LaneDepartureChecker::calcMaxSearchLengthForBoundaries(const Trajectory & trajectory) const { - for (const auto & candidate_lanelet : candidate_lanelets) { - const std::string r_type = - candidate_lanelet.rightBound().attributeOr(lanelet::AttributeName::Type, "none"); - if ( - std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), r_type) != - boundary_types_to_detect.end()) { - if (isCrossingWithBoundary( - candidate_lanelet.rightBound2d().basicLineString(), vehicle_footprints)) { - // std::cerr << "The crossed road_border's line string id: " - // << candidate_lanelet.rightBound().id() << std::endl; - return true; + const double max_ego_lon_length = std::max( + std::abs(vehicle_info_ptr_->max_longitudinal_offset_m), + std::abs(vehicle_info_ptr_->min_longitudinal_offset_m)); + const double max_ego_lat_length = std::max( + std::abs(vehicle_info_ptr_->max_lateral_offset_m), + std::abs(vehicle_info_ptr_->min_lateral_offset_m)); + const double max_ego_search_length = std::hypot(max_ego_lon_length, max_ego_lat_length); + return calcArcLength(trajectory.points) + max_ego_search_length; +} + +SegmentRtree LaneDepartureChecker::extractUncrossableBoundaries( + const lanelet::LaneletMap & lanelet_map, const geometry_msgs::msg::Point & ego_point, + const double max_search_length, const std::vector & boundary_types_to_detect) +{ + const auto has_types = + [](const lanelet::ConstLineString3d & ls, const std::vector & types) { + constexpr auto no_type = ""; + const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); + return (type != no_type && std::find(types.begin(), types.end(), type) != types.end()); + }; + + SegmentRtree uncrossable_segments_in_range; + LineString2d line; + const auto ego_p = Point2d{ego_point.x, ego_point.y}; + for (const auto & ls : lanelet_map.lineStringLayer) { + if (has_types(ls, boundary_types_to_detect)) { + line.clear(); + for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); + for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) { + const Segment2d segment = {line[segment_idx], line[segment_idx + 1]}; + if (boost::geometry::distance(segment, ego_p) < max_search_length) { + uncrossable_segments_in_range.insert(segment); + } } } - const std::string l_type = - candidate_lanelet.leftBound().attributeOr(lanelet::AttributeName::Type, "none"); - if ( - std::find(boundary_types_to_detect.begin(), boundary_types_to_detect.end(), l_type) != - boundary_types_to_detect.end()) { - if (isCrossingWithBoundary( - candidate_lanelet.leftBound2d().basicLineString(), vehicle_footprints)) { - // std::cerr << "The crossed road_border's line string id: " - // << candidate_lanelet.leftBound().id() << std::endl; - return true; - } + } + return uncrossable_segments_in_range; +} + +bool LaneDepartureChecker::willCrossBoundary( + const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments) +{ + for (const auto & footprint : vehicle_footprints) { + std::vector intersection_result; + uncrossable_segments.query( + boost::geometry::index::intersects(footprint), std::back_inserter(intersection_result)); + if (!intersection_result.empty()) { + return true; } } return false; } - } // namespace lane_departure_checker diff --git a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml index 59fd5174fa38c..23a1201a84958 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml @@ -33,4 +33,12 @@ + + + + + + + + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 1126914d58c5a..c4de9c04dcaf2 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -25,6 +25,7 @@ ndt_scan_matcher pointcloud_preprocessor pose_initializer + pose_instability_detector topic_tools yabloc_common yabloc_image_processing diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 20b1b5a4d0b27..1b4c3f38e038d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -50,6 +50,7 @@ + @@ -78,9 +79,7 @@ - - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 33994934949ac..b69f1c0ee8b14 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -32,6 +32,7 @@ + @@ -145,6 +146,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index cee6829ea3b4a..ed37f6270c913 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -11,6 +11,7 @@ + @@ -37,9 +38,7 @@ - - - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 5b5646a061ac7..26e7af07646cd 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -6,10 +6,8 @@ - - - + @@ -43,14 +41,6 @@ - - - - - - - - - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 8b241db9774b3..6582dfd32d91d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -12,6 +12,7 @@ + @@ -103,7 +104,7 @@ @@ -175,7 +176,9 @@ + + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 336588891d9b2..7e6ba83747a8b 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -39,6 +39,7 @@ shape_estimation tensorrt_yolo topic_tools + tracking_object_merger traffic_light_arbiter traffic_light_classifier traffic_light_fine_detector diff --git a/localization/geo_pose_projector/src/geo_pose_projector.cpp b/localization/geo_pose_projector/src/geo_pose_projector.cpp index 6d5308b929b25..d147888bb743b 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.cpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.cpp @@ -36,7 +36,8 @@ GeoPoseProjector::GeoPoseProjector() // Subscribe to geo_pose topic geo_pose_sub_ = create_subscription( - "input_geo_pose", 10, [this](const GeoPoseWithCovariance::SharedPtr msg) { on_geo_pose(msg); }); + "input_geo_pose", 10, + [this](const GeoPoseWithCovariance::ConstSharedPtr msg) { on_geo_pose(msg); }); // Publish pose topic pose_pub_ = create_publisher("output_pose", 10); @@ -49,7 +50,7 @@ GeoPoseProjector::GeoPoseProjector() } } -void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg) +void GeoPoseProjector::on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg) { if (!projector_info_) { RCLCPP_WARN_THROTTLE( diff --git a/localization/geo_pose_projector/src/geo_pose_projector.hpp b/localization/geo_pose_projector/src/geo_pose_projector.hpp index b24b976b1eb61..b0611fec36984 100644 --- a/localization/geo_pose_projector/src/geo_pose_projector.hpp +++ b/localization/geo_pose_projector/src/geo_pose_projector.hpp @@ -39,7 +39,7 @@ class GeoPoseProjector : public rclcpp::Node GeoPoseProjector(); private: - void on_geo_pose(const GeoPoseWithCovariance::SharedPtr msg); + void on_geo_pose(const GeoPoseWithCovariance::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Subscription::SharedPtr geo_pose_sub_; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 24b737c309016..7e83220dadf2a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -13,6 +13,7 @@ ament_cmake autoware_cmake + ament_index_cpp aruco cv_bridge diagnostic_msgs diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp index 6b302ee6bc862..1b44327fd9e8b 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -14,6 +14,7 @@ #include "../src/ar_tag_based_localizer.hpp" +#include #include #include @@ -29,8 +30,8 @@ class TestArTagBasedLocalizer : public ::testing::Test void SetUp() override { const std::string yaml_path = - "../../install/ar_tag_based_localizer/share/ar_tag_based_localizer/config/" - "ar_tag_based_localizer.param.yaml"; + ament_index_cpp::get_package_share_directory("ar_tag_based_localizer") + + "/config/ar_tag_based_localizer.param.yaml"; rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { diff --git a/localization/pose_instability_detector/CMakeLists.txt b/localization/pose_instability_detector/CMakeLists.txt new file mode 100644 index 0000000000000..5270df636d791 --- /dev/null +++ b/localization/pose_instability_detector/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.14) +project(pose_instability_detector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(pose_instability_detector + src/main.cpp + src/pose_instability_detector.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_pose_instability_detector + test/test.cpp + src/pose_instability_detector.cpp + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md new file mode 100644 index 0000000000000..89cf6ca3be684 --- /dev/null +++ b/localization/pose_instability_detector/README.md @@ -0,0 +1,37 @@ +# pose_instability_detector + +The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). + +This node triggers periodic timer callbacks to compare two poses: + +- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`. +- The latest pose from `/localization/kinematic_state`. + +The results of this comparison are then output to the `/diagnostics` topic. + +If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values. +This discrepancy suggests that there may be an issue with either the estimated pose or the input twist. + +The following diagram provides an overview of what the timeline of this process looks like: + +![timeline](./media/timeline.drawio.svg) + +## Parameters + +{{ json_to_markdown("localization/pose_instability_detector/schema/pose_instability_detector.schema.json") }} + +## Input + +| Name | Type | Description | +| ------------------ | ---------------------------------------------- | --------------------- | +| `~/input/odometry` | nav_msgs::msg::Odometry | Pose estimated by EKF | +| `~/input/twist` | geometry_msgs::msg::TwistWithCovarianceStamped | Twist | + +## Output + +| Name | Type | Description | +| ------------------- | ------------------------------------- | ----------- | +| `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics | + +![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml new file mode 100644 index 0000000000000..29a25849d6b1c --- /dev/null +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + interval_sec: 1.0 # [sec] + threshold_diff_position_x: 1.0 # [m] + threshold_diff_position_y: 1.0 # [m] + threshold_diff_position_z: 1.0 # [m] + threshold_diff_angle_x: 1.0 # [rad] + threshold_diff_angle_y: 1.0 # [rad] + threshold_diff_angle_z: 1.0 # [rad] diff --git a/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml new file mode 100644 index 0000000000000..4a390ee2854d7 --- /dev/null +++ b/localization/pose_instability_detector/launch/pose_instability_detector.launch.xml @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png new file mode 100644 index 0000000000000..b3ad402e48ba7 Binary files /dev/null and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/media/timeline.drawio.svg b/localization/pose_instability_detector/media/timeline.drawio.svg new file mode 100644 index 0000000000000..8fcdef3401025 --- /dev/null +++ b/localization/pose_instability_detector/media/timeline.drawio.svg @@ -0,0 +1,157 @@ + + + + + + + + Time + + + + + + timer_callback + + + + + + timer_callback + + + + + + prev_ekf + + + + + + latest_ekf + + + + + + ekf(ignored) + + + + + 1 sec + + + + + + ekf(ignored) + + + + + + ekf(ignored) + + + + ... + + + + About ekf + + + + + + Time + + + + + + timer_callback + + + + + + timer_callback + + + + + + prev_ekf + + + + + + latest_ekf + + + + + + twist + + + + + 1 sec + + + + + + twist + + + + + + twist + + + + ... + + + + About twist + + + + + + twist + (ignored) + + + diff --git a/localization/pose_instability_detector/package.xml b/localization/pose_instability_detector/package.xml new file mode 100644 index 0000000000000..7774407a7990d --- /dev/null +++ b/localization/pose_instability_detector/package.xml @@ -0,0 +1,35 @@ + + + + pose_instability_detector + 0.1.0 + The pose_instability_detector package + Yamato Ando + Kento Yabuuchi + Masahiro Sakamoto + Taiki Yamada + Ryu Yamamoto + Shintaro Sakoda + Apache License 2.0 + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + ament_index_cpp + diagnostic_msgs + geometry_msgs + nav_msgs + rclcpp + tf2 + tf2_geometry_msgs + tier4_autoware_utils + tier4_debug_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json new file mode 100644 index 0000000000000..b45b43102444d --- /dev/null +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -0,0 +1,75 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Pose Instability Detector Nodes", + "type": "object", + "definitions": { + "pose_instability_detector_node": { + "type": "object", + "properties": { + "interval_sec": { + "type": "number", + "default": 1.0, + "exclusiveMinimum": 0, + "description": "The interval of timer_callback in seconds." + }, + "threshold_diff_position_x": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position x (m)." + }, + "threshold_diff_position_y": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position y (m)." + }, + "threshold_diff_position_z": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_position z (m)." + }, + "threshold_diff_angle_x": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle x (rad)." + }, + "threshold_diff_angle_y": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle y (rad)." + }, + "threshold_diff_angle_z": { + "type": "number", + "default": 1.0, + "minimum": 0.0, + "description": "The threshold of diff_angle z (rad)." + } + }, + "required": [ + "interval_sec", + "threshold_diff_position_x", + "threshold_diff_position_y", + "threshold_diff_position_z", + "threshold_diff_angle_x", + "threshold_diff_angle_y", + "threshold_diff_angle_z" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/pose_instability_detector_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/localization/pose_instability_detector/src/main.cpp b/localization/pose_instability_detector/src/main.cpp new file mode 100644 index 0000000000000..34e679e86730f --- /dev/null +++ b/localization/pose_instability_detector/src/main.cpp @@ -0,0 +1,26 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_instability_detector.hpp" + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto pose_instability_detector = std::make_shared(); + rclcpp::spin(pose_instability_detector); + rclcpp::shutdown(); + return 0; +} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp new file mode 100644 index 0000000000000..afb7d6e007db2 --- /dev/null +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -0,0 +1,176 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose_instability_detector.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +#include + +PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) +: Node("pose_instability_detector", options), + threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), + threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), + threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), + threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), + threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), + threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) +{ + odometry_sub_ = this->create_subscription( + "~/input/odometry", 10, + std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); + + twist_sub_ = this->create_subscription( + "~/input/twist", 10, + std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); + + const double interval_sec = this->declare_parameter("interval_sec"); + timer_ = rclcpp::create_timer( + this, this->get_clock(), std::chrono::duration(interval_sec), + std::bind(&PoseInstabilityDetector::callback_timer, this)); + + diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); + diagnostics_pub_ = this->create_publisher("/diagnostics", 10); +} + +void PoseInstabilityDetector::callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr) +{ + latest_odometry_ = *odometry_msg_ptr; +} + +void PoseInstabilityDetector::callback_twist( + TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + twist_buffer_.push_back(*twist_msg_ptr); +} + +void PoseInstabilityDetector::callback_timer() +{ + if (latest_odometry_ == std::nullopt) { + return; + } + if (prev_odometry_ == std::nullopt) { + prev_odometry_ = latest_odometry_; + return; + } + + auto quat_to_rpy = [](const Quaternion & quat) { + tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); + tf2::Matrix3x3 mat(tf2_quat); + double roll{}; + double pitch{}; + double yaw{}; + mat.getRPY(roll, pitch, yaw); + return std::make_tuple(roll, pitch, yaw); + }; + + Pose pose = prev_odometry_->pose.pose; + rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); + for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { + const Twist twist = twist_with_cov.twist.twist; + + const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); + if (curr_time > latest_odometry_->header.stamp) { + break; + } + + const rclcpp::Duration time_diff = curr_time - prev_time; + const double time_diff_sec = time_diff.seconds(); + if (time_diff_sec < 0.0) { + continue; + } + + // quat to rpy + auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); + + // rpy update + ang_x += twist.angular.x * time_diff_sec; + ang_y += twist.angular.y * time_diff_sec; + ang_z += twist.angular.z * time_diff_sec; + tf2::Quaternion quat; + quat.setRPY(ang_x, ang_y, ang_z); + + // Convert twist to world frame + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(quat, linear_velocity); + + // update + pose.position.x += linear_velocity.x() * time_diff_sec; + pose.position.y += linear_velocity.y() * time_diff_sec; + pose.position.z += linear_velocity.z() * time_diff_sec; + pose.orientation.x = quat.x(); + pose.orientation.y = quat.y(); + pose.orientation.z = quat.z(); + pose.orientation.w = quat.w(); + prev_time = curr_time; + } + + // compare pose and latest_odometry_ + const Pose latest_ekf_pose = latest_odometry_->pose.pose; + const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_odom.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation); + const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; + + const rclcpp::Time stamp = latest_odometry_->header.stamp; + + // publish diff_pose for debug + PoseStamped diff_pose; + diff_pose.header = latest_odometry_->header; + diff_pose.pose = ekf_to_odom; + diff_pose_pub_->publish(diff_pose); + + const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, + threshold_diff_position_z_, threshold_diff_angle_x_, + threshold_diff_angle_y_, threshold_diff_angle_z_}; + + const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", + "diff_angle_x", "diff_angle_y", "diff_angle_z"}; + + // publish diagnostics + DiagnosticStatus status; + status.name = "localization: pose_instability_detector"; + status.hardware_id = this->get_name(); + bool all_ok = true; + + for (size_t i = 0; i < values.size(); ++i) { + const bool ok = (std::abs(values[i]) < thresholds[i]); + all_ok &= ok; + diagnostic_msgs::msg::KeyValue kv; + kv.key = labels[i] + ":threshold"; + kv.value = std::to_string(thresholds[i]); + status.values.push_back(kv); + kv.key = labels[i] + ":value"; + kv.value = std::to_string(values[i]); + status.values.push_back(kv); + kv.key = labels[i] + ":status"; + kv.value = (ok ? "OK" : "WARN"); + status.values.push_back(kv); + } + status.level = (all_ok ? DiagnosticStatus::OK : DiagnosticStatus::WARN); + status.message = (all_ok ? "OK" : "WARN"); + + DiagnosticArray diagnostics; + diagnostics.header.stamp = stamp; + diagnostics.status.emplace_back(status); + diagnostics_pub_->publish(diagnostics); + + // prepare for next loop + prev_odometry_ = latest_odometry_; + twist_buffer_.clear(); +} diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/src/pose_instability_detector.hpp new file mode 100644 index 0000000000000..761a10b7a6bf7 --- /dev/null +++ b/localization/pose_instability_detector/src/pose_instability_detector.hpp @@ -0,0 +1,70 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POSE_INSTABILITY_DETECTOR_HPP_ +#define POSE_INSTABILITY_DETECTOR_HPP_ + +#include + +#include +#include +#include +#include + +#include + +class PoseInstabilityDetector : public rclcpp::Node +{ + using Quaternion = geometry_msgs::msg::Quaternion; + using Twist = geometry_msgs::msg::Twist; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Pose = geometry_msgs::msg::Pose; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using Odometry = nav_msgs::msg::Odometry; + using KeyValue = diagnostic_msgs::msg::KeyValue; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +private: + void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); + void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + void callback_timer(); + + // subscribers and timer + rclcpp::Subscription::SharedPtr odometry_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + // publisher + rclcpp::Publisher::SharedPtr diff_pose_pub_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + // parameters + const double threshold_diff_position_x_; + const double threshold_diff_position_y_; + const double threshold_diff_position_z_; + const double threshold_diff_angle_x_; + const double threshold_diff_angle_y_; + const double threshold_diff_angle_z_; + + // variables + std::optional latest_odometry_ = std::nullopt; + std::optional prev_odometry_ = std::nullopt; + std::vector twist_buffer_; +}; + +#endif // POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp new file mode 100644 index 0000000000000..5ea03859d7731 --- /dev/null +++ b/localization/pose_instability_detector/test/test.cpp @@ -0,0 +1,169 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/pose_instability_detector.hpp" +#include "test_message_helper_node.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +class TestPoseInstabilityDetector : public ::testing::Test +{ + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Odometry = nav_msgs::msg::Odometry; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +protected: + void SetUp() override + { + const std::string yaml_path = + ament_index_cpp::get_package_share_directory("pose_instability_detector") + + "/config/pose_instability_detector.param.yaml"; + + rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); + if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { + std::cerr << "Failed to parse yaml file : " << yaml_path << std::endl; + std::exit(1); + } + + const rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(params_st, ""); + rclcpp::NodeOptions node_options; + for (const auto & param_pair : param_map) { + for (const auto & param : param_pair.second) { + node_options.parameter_overrides().push_back(param); + } + } + + subject_ = std::make_shared(node_options); + executor_.add_node(subject_); + + helper_ = std::make_shared(); + executor_.add_node(helper_); + + rcl_yaml_node_struct_fini(params_st); + } + + void TearDown() override + { + executor_.remove_node(subject_); + executor_.remove_node(helper_); + } + + rclcpp::executors::SingleThreadedExecutor executor_; + std::shared_ptr subject_; + std::shared_ptr helper_; +}; + +TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // NOLINT +{ + // send the first odometry message (start x = 10) + builtin_interfaces::msg::Time timestamp{}; + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + + // send the twist message2 (move 1m in x direction) + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + + // send the second odometry message (finish x = 12) + timestamp.sec = 2; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // check result + const diagnostic_msgs::msg::DiagnosticStatus & diagnostic_status = + helper_->received_diagnostic_array.status[0]; + EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOLINT +{ + // send the first odometry message (start x = 10) + builtin_interfaces::msg::Time timestamp{}; + timestamp.sec = 0; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + + // process the above message (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the twist message2 (move 0.1m in x direction) + timestamp.sec = 1; + timestamp.nanosec = 0; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + + // send the second odometry message (finish x = 12) + timestamp.sec = 2; + timestamp.nanosec = 0; + helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + + // process the above messages (by timer_callback) + helper_->received_diagnostic_array_flag = false; + while (!helper_->received_diagnostic_array_flag) { + executor_.spin_some(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // check result + const diagnostic_msgs::msg::DiagnosticStatus & diagnostic_status = + helper_->received_diagnostic_array.status[0]; + EXPECT_TRUE(diagnostic_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/pose_instability_detector/test/test_message_helper_node.hpp b/localization/pose_instability_detector/test/test_message_helper_node.hpp new file mode 100644 index 0000000000000..51444f9736b02 --- /dev/null +++ b/localization/pose_instability_detector/test/test_message_helper_node.hpp @@ -0,0 +1,80 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TEST_MESSAGE_HELPER_NODE_HPP_ +#define TEST_MESSAGE_HELPER_NODE_HPP_ + +#include + +#include +#include +#include + +class TestMessageHelperNode : public rclcpp::Node +{ + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Odometry = nav_msgs::msg::Odometry; + using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; + using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; + +public: + TestMessageHelperNode() : Node("test_message_helper_node") + { + odometry_publisher_ = + this->create_publisher("/pose_instability_detector/input/odometry", 10); + twist_publisher_ = this->create_publisher( + "/pose_instability_detector/input/twist", 10); + diagnostic_subscriber_ = this->create_subscription( + "/diagnostics", 10, + std::bind(&TestMessageHelperNode::callback_diagnostics, this, std::placeholders::_1)); + } + + void send_odometry_message( + const builtin_interfaces::msg::Time timestamp, const double x, const double y, const double z) + { + Odometry message{}; + message.header.stamp = timestamp; + message.pose.pose.position.x = x; + message.pose.pose.position.y = y; + message.pose.pose.position.z = z; + odometry_publisher_->publish(message); + } + + void send_twist_message( + const builtin_interfaces::msg::Time timestamp, const double x, const double y, const double z) + { + TwistWithCovarianceStamped message{}; + message.header.stamp = timestamp; + message.twist.twist.linear.x = x; + message.twist.twist.linear.y = y; + message.twist.twist.linear.z = z; + twist_publisher_->publish(message); + } + + void callback_diagnostics(const DiagnosticArray::ConstSharedPtr msg) + { + received_diagnostic_array = *msg; + received_diagnostic_array_flag = true; + } + + DiagnosticArray received_diagnostic_array; + bool received_diagnostic_array_flag = false; + +private: + rclcpp::Publisher::SharedPtr odometry_publisher_; + rclcpp::Publisher::SharedPtr twist_publisher_; + rclcpp::Subscription::SharedPtr diagnostic_subscriber_; +}; + +#endif // TEST_MESSAGE_HELPER_NODE_HPP_ diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index 745a0fd6591a9..f7a27a52bfa8a 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -11,3 +11,5 @@ min_points_and_distance_ratio: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] + + using_2d_validator: false 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 9ef1f75427b65..6597475ae297e 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 @@ -71,11 +71,18 @@ class Debugger void addNeighborPointcloud(const pcl::PointCloud::Ptr & input) { pcl::PointCloud::Ptr input_xyz = toXYZ(input); - for (const auto & point : *input_xyz) { - neighbor_pointcloud_->push_back(point); - } + addNeighborPointcloud(input_xyz); } + void addNeighborPointcloud(const pcl::PointCloud::Ptr & input) + { + if (!input->empty()) { + neighbor_pointcloud_->reserve(neighbor_pointcloud_->size() + input->size()); + for (const auto & point : *input) { + neighbor_pointcloud_->push_back(point); + } + } + } void addPointcloudWithinPolygon(const pcl::PointCloud::Ptr & input) { // pcl::PointCloud::Ptr input_xyz = toXYZ(input); diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 5819302664907..e7df2c409b18f 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -25,8 +25,13 @@ #include #include #include +#include +#include #include #include +#include +#include +#include #include #include @@ -35,12 +40,88 @@ #include namespace obstacle_pointcloud_based_validator { + struct PointsNumThresholdParam { std::vector min_points_num; std::vector max_points_num; std::vector min_points_and_distance_ratio; }; + +class Validator +{ +private: + PointsNumThresholdParam points_num_threshold_param_; + +protected: + pcl::PointCloud::Ptr cropped_pointcloud_; + +public: + explicit Validator(PointsNumThresholdParam & points_num_threshold_param); + inline pcl::PointCloud::Ptr getDebugPointCloudWithinObject() + { + return cropped_pointcloud_; + } + + virtual bool setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud) = 0; + virtual bool validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object) = 0; + virtual std::optional getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object) = 0; + size_t getThresholdPointCloud(const autoware_auto_perception_msgs::msg::DetectedObject & object); + virtual pcl::PointCloud::Ptr getDebugNeighborPointCloud() = 0; +}; + +class Validator2D : public Validator +{ +private: + pcl::PointCloud::Ptr obstacle_pointcloud_; + pcl::PointCloud::Ptr neighbor_pointcloud_; + pcl::search::Search::Ptr kdtree_; + +public: + explicit Validator2D(PointsNumThresholdParam & points_num_threshold_param); + + pcl::PointCloud::Ptr convertToXYZ( + const pcl::PointCloud::Ptr & pointcloud_xy); + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + { + return convertToXYZ(neighbor_pointcloud_); + } + + bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); + bool validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object); + std::optional getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object); + std::optional getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr neighbor_pointcloud); +}; +class Validator3D : public Validator +{ +private: + pcl::PointCloud::Ptr obstacle_pointcloud_; + pcl::PointCloud::Ptr neighbor_pointcloud_; + pcl::search::Search::Ptr kdtree_; + +public: + explicit Validator3D(PointsNumThresholdParam & points_num_threshold_param); + inline pcl::PointCloud::Ptr getDebugNeighborPointCloud() + { + return neighbor_pointcloud_; + } + bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud); + bool validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object); + std::optional getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object); + std::optional getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr neighbor_pointcloud); +}; + class ObstaclePointCloudBasedValidator : public rclcpp::Node { public: @@ -61,16 +142,13 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node PointsNumThresholdParam points_num_threshold_param_; std::shared_ptr debugger_; + bool using_2d_validator_; + std::unique_ptr validator_; private: void onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud); - std::optional getPointCloudNumWithinPolygon( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr pointcloud); - std::optional getMaxRadius( - const autoware_auto_perception_msgs::msg::DetectedObject & object); }; } // namespace obstacle_pointcloud_based_validator diff --git a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md index dd67aab3db0c9..36b3815e7e689 100644 --- a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md +++ b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md @@ -28,6 +28,7 @@ In the debug image above, the red DetectedObject is the validated object. The bl | Name | Type | Description | | ------------------------------- | ----- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `using_2d_validator` | bool | The xy-plane projected (2D) obstacle point clouds will be used for validation | | `min_points_num` | int | The minimum number of obstacle point clouds in DetectedObjects | | `max_points_num` | int | The max number of obstacle point clouds in DetectedObjects | | `min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index e19b2b1c399b2..31bb633294748 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -6,6 +6,7 @@ The ROS 2 detected_object_validation package Yukihiro Saito Shunsuke Miura + badai nguyen Apache License 2.0 ament_cmake_auto 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 47640c9332e4d..3249581513dd9 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -19,12 +19,6 @@ #include -#include -#include -#include -#include -#include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -35,44 +29,247 @@ #include #include -namespace +namespace obstacle_pointcloud_based_validator { -inline pcl::PointXY toPCL(const double x, const double y) +namespace bg = boost::geometry; +using Shape = autoware_auto_perception_msgs::msg::Shape; +using Polygon2d = tier4_autoware_utils::Polygon2d; + +Validator::Validator(PointsNumThresholdParam & points_num_threshold_param) { - pcl::PointXY pcl_point; - pcl_point.x = x; - pcl_point.y = y; - return pcl_point; + points_num_threshold_param_.min_points_num = points_num_threshold_param.min_points_num; + points_num_threshold_param_.max_points_num = points_num_threshold_param.max_points_num; + points_num_threshold_param_.min_points_and_distance_ratio = + points_num_threshold_param.min_points_and_distance_ratio; } -inline pcl::PointXY toPCL(const geometry_msgs::msg::Point & point) +size_t Validator::getThresholdPointCloud( + const autoware_auto_perception_msgs::msg::DetectedObject & object) { - return toPCL(point.x, point.y); + const auto object_label_id = object.classification.front().label; + const auto object_distance = std::hypot( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + size_t threshold_pc = std::clamp( + static_cast( + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); + return threshold_pc; } -inline pcl::PointXYZ toXYZ(const pcl::PointXY & point) +Validator2D::Validator2D(PointsNumThresholdParam & points_num_threshold_param) +: Validator(points_num_threshold_param) { - return pcl::PointXYZ(point.x, point.y, 0.0); } -inline pcl::PointCloud::Ptr toXYZ( - const pcl::PointCloud::Ptr & pointcloud) +bool Validator2D::setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) +{ + obstacle_pointcloud_.reset(new pcl::PointCloud); + pcl::fromROSMsg(*input_cloud, *obstacle_pointcloud_); + if (obstacle_pointcloud_->empty()) { + return false; + } + + kdtree_ = pcl::make_shared>(false); + kdtree_->setInputCloud(obstacle_pointcloud_); + + return true; +} +pcl::PointCloud::Ptr Validator2D::convertToXYZ( + const pcl::PointCloud::Ptr & pointcloud_xy) { pcl::PointCloud::Ptr pointcloud_xyz(new pcl::PointCloud); - pointcloud_xyz->reserve(pointcloud->size()); - for (const auto & point : *pointcloud) { - pointcloud_xyz->push_back(toXYZ(point)); + pointcloud_xyz->reserve(pointcloud_xy->size()); + for (const auto & point : *pointcloud_xy) { + pointcloud_xyz->push_back(pcl::PointXYZ(point.x, point.y, 0.0)); } return pointcloud_xyz; } -} // namespace +std::optional Validator2D::getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr pointcloud) +{ + std::vector vertices_array; + pcl::Vertices vertices; + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + if (bg::is_empty(poly2d)) return std::nullopt; -namespace obstacle_pointcloud_based_validator + pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + + for (size_t i = 0; i < poly2d.outer().size(); ++i) { + vertices.vertices.emplace_back(i); + vertices_array.emplace_back(vertices); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); + } + cropped_pointcloud_.reset(new pcl::PointCloud); + pcl::CropHull cropper; // don't be implemented PointXY by PCL + cropper.setInputCloud(convertToXYZ(pointcloud)); + cropper.setDim(2); + cropper.setHullIndices(vertices_array); + cropper.setHullCloud(poly3d); + cropper.setCropOutside(true); + cropper.filter(*cropped_pointcloud_); + return cropped_pointcloud_->size(); +} + +bool Validator2D::validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object) { -namespace bg = boost::geometry; -using Shape = autoware_auto_perception_msgs::msg::Shape; -using Polygon2d = tier4_autoware_utils::Polygon2d; + // get neighbor_pointcloud of object + neighbor_pointcloud_.reset(new pcl::PointCloud); + std::vector indices; + std::vector distances; + const auto search_radius = getMaxRadius(transformed_object); + if (!search_radius) { + return false; + } + kdtree_->radiusSearch( + pcl::PointXY( + transformed_object.kinematics.pose_with_covariance.pose.position.x, + transformed_object.kinematics.pose_with_covariance.pose.position.y), + search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud_->push_back(obstacle_pointcloud_->at(index)); + } + const auto num = getPointCloudWithinObject(transformed_object, neighbor_pointcloud_); + if (!num) return true; + + size_t threshold_pointcloud_num = getThresholdPointCloud(transformed_object); + if (num.value() > threshold_pointcloud_num) { + return true; + } + return false; // remove object +} + +std::optional Validator2D::getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { + return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); + } else if (object.shape.type == Shape::POLYGON) { + float max_dist = 0.0; + for (const auto & point : object.shape.footprint.points) { + const float dist = std::hypot(point.x, point.y); + max_dist = max_dist < dist ? dist : max_dist; + } + return max_dist; + } else { + return std::nullopt; + } +} + +Validator3D::Validator3D(PointsNumThresholdParam & points_num_threshold_param) +: Validator(points_num_threshold_param) +{ +} +bool Validator3D::setKdtreeInputCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud) +{ + obstacle_pointcloud_.reset(new pcl::PointCloud); + pcl::fromROSMsg(*input_cloud, *obstacle_pointcloud_); + if (obstacle_pointcloud_->empty()) { + return false; + } + // setup kdtree_ + kdtree_ = pcl::make_shared>(false); + kdtree_->setInputCloud(obstacle_pointcloud_); + return true; +} +std::optional Validator3D::getMaxRadius( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { + auto square_radius = (object.shape.dimensions.x * 0.5f) * (object.shape.dimensions.x * 0.5f) + + (object.shape.dimensions.y * 0.5f) * (object.shape.dimensions.y * 0.5f) + + (object.shape.dimensions.z * 0.5f) * (object.shape.dimensions.z * 0.5f); + return std::sqrt(square_radius); + } else if (object.shape.type == Shape::POLYGON) { + float max_dist = 0.0; + for (const auto & point : object.shape.footprint.points) { + const float dist = std::hypot(point.x, point.y); + max_dist = max_dist < dist ? dist : max_dist; + } + return std::hypot(max_dist, object.shape.dimensions.z * 0.5f); + } else { + return std::nullopt; + } +} + +std::optional Validator3D::getPointCloudWithinObject( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const pcl::PointCloud::Ptr neighbor_pointcloud) +{ + std::vector vertices_array; + pcl::Vertices vertices; + auto const & object_position = object.kinematics.pose_with_covariance.pose.position; + auto const object_height = object.shape.dimensions.x; + auto z_min = object_position.z - object_height / 2.0f; + auto z_max = object_position.z + object_height / 2.0f; + Polygon2d poly2d = + tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); + if (bg::is_empty(poly2d)) return std::nullopt; + + pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); + for (size_t i = 0; i < poly2d.outer().size(); ++i) { + vertices.vertices.emplace_back(i); + vertices_array.emplace_back(vertices); + poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); + } + + pcl::PointCloud::Ptr cropped_pointcloud_2d(new pcl::PointCloud); + pcl::CropHull cropper; + cropper.setInputCloud(neighbor_pointcloud); + cropper.setDim(2); + cropper.setHullIndices(vertices_array); + cropper.setHullCloud(poly3d); + cropper.setCropOutside(true); + cropper.filter(*cropped_pointcloud_2d); + + cropped_pointcloud_.reset(new pcl::PointCloud); + cropped_pointcloud_->reserve(cropped_pointcloud_2d->size()); + for (const auto & point : *cropped_pointcloud_2d) { + if (point.z > z_min && point.z < z_max) { + cropped_pointcloud_->push_back(point); + } + } + return cropped_pointcloud_->size(); +} + +bool Validator3D::validate_object( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object) +{ + neighbor_pointcloud_.reset(new pcl::PointCloud); + std::vector indices; + std::vector distances; + const auto search_radius = getMaxRadius(transformed_object); + if (!search_radius) { + return false; + } + kdtree_->radiusSearch( + pcl::PointXYZ( + transformed_object.kinematics.pose_with_covariance.pose.position.x, + transformed_object.kinematics.pose_with_covariance.pose.position.y, + transformed_object.kinematics.pose_with_covariance.pose.position.z), + search_radius.value(), indices, distances); + for (const auto & index : indices) { + neighbor_pointcloud_->push_back(obstacle_pointcloud_->at(index)); + } + + const auto num = getPointCloudWithinObject(transformed_object, neighbor_pointcloud_); + if (!num) return true; + + size_t threshold_pointcloud_num = getThresholdPointCloud(transformed_object); + if (num.value() > threshold_pointcloud_num) { + return true; + } + return false; // remove object +} ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( const rclcpp::NodeOptions & node_options) @@ -85,13 +282,6 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( tf_listener_(tf_buffer_), sync_(SyncPolicy(10), objects_sub_, obstacle_pointcloud_sub_) { - using std::placeholders::_1; - using std::placeholders::_2; - sync_.registerCallback( - std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); - objects_pub_ = create_publisher( - "~/output/objects", rclcpp::QoS{1}); - points_num_threshold_param_.min_points_num = declare_parameter>("min_points_num"); points_num_threshold_param_.max_points_num = @@ -99,10 +289,25 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( points_num_threshold_param_.min_points_and_distance_ratio = declare_parameter>("min_points_and_distance_ratio"); + using_2d_validator_ = declare_parameter("using_2d_validator"); + + using std::placeholders::_1; + using std::placeholders::_2; + + sync_.registerCallback( + std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2)); + if (using_2d_validator_) { + validator_ = std::make_unique(points_num_threshold_param_); + } else { + validator_ = std::make_unique(points_num_threshold_param_); + } + + objects_pub_ = create_publisher( + "~/output/objects", rclcpp::QoS{1}); + const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); } - void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects, const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud) @@ -119,61 +324,23 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( // objects_pub_->publish(*input_objects); return; } - - // Convert to PCL - pcl::PointCloud::Ptr obstacle_pointcloud(new pcl::PointCloud); - pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud); - if (obstacle_pointcloud->empty()) { + if (!validator_->setKdtreeInputCloud(input_obstacle_pointcloud)) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud"); - // objects_pub_->publish(*input_objects); return; } - // Create Kd-tree to search neighbor pointcloud to reduce cost. - pcl::search::Search::Ptr kdtree = - pcl::make_shared>(false); - kdtree->setInputCloud(obstacle_pointcloud); - for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); - const auto object_label_id = transformed_object.classification.front().label; const auto & object = input_objects->objects.at(i); - const auto & transformed_object_position = - transformed_object.kinematics.pose_with_covariance.pose.position; - const auto search_radius = getMaxRadius(transformed_object); - if (!search_radius) { - output.objects.push_back(object); - continue; - } - - // Search neighbor pointcloud to reduce cost. - pcl::PointCloud::Ptr neighbor_pointcloud(new pcl::PointCloud); - std::vector indices; - std::vector distances; - kdtree->radiusSearch( - toPCL(transformed_object_position), search_radius.value(), indices, distances); - for (const auto & index : indices) { - neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); + const auto validated = validator_->validate_object(transformed_object); + if (debugger_) { + debugger_->addNeighborPointcloud(validator_->getDebugNeighborPointCloud()); + debugger_->addPointcloudWithinPolygon(validator_->getDebugPointCloudWithinObject()); } - if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); - - // Filter object that have few pointcloud in them. - // TODO(badai-nguyen) add 3d validator option - const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); - const auto object_distance = - std::hypot(transformed_object_position.x, transformed_object_position.y); - size_t min_pointcloud_num = std::clamp( - static_cast( - points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / - object_distance + - 0.5f), - static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), - static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); - if (num) { - (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); - } else { + if (validated) { output.objects.push_back(object); + } else { + removed_objects.objects.push_back(object); } } @@ -185,56 +352,6 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( } } -std::optional ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon( - const autoware_auto_perception_msgs::msg::DetectedObject & object, - const pcl::PointCloud::Ptr pointcloud) -{ - pcl::PointCloud::Ptr cropped_pointcloud(new pcl::PointCloud); - std::vector vertices_array; - pcl::Vertices vertices; - - Polygon2d poly2d = - tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape); - if (bg::is_empty(poly2d)) return std::nullopt; - - pcl::PointCloud::Ptr poly3d(new pcl::PointCloud); - - for (size_t i = 0; i < poly2d.outer().size(); ++i) { - vertices.vertices.emplace_back(i); - vertices_array.emplace_back(vertices); - poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), 0.0); - } - - pcl::CropHull cropper; // don't be implemented PointXY by PCL - cropper.setInputCloud(toXYZ(pointcloud)); - cropper.setDim(2); - cropper.setHullIndices(vertices_array); - cropper.setHullCloud(poly3d); - cropper.setCropOutside(true); - cropper.filter(*cropped_pointcloud); - - if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud); - return cropped_pointcloud->size(); -} - -std::optional ObstaclePointCloudBasedValidator::getMaxRadius( - const autoware_auto_perception_msgs::msg::DetectedObject & object) -{ - if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) { - return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f); - } else if (object.shape.type == Shape::POLYGON) { - float max_dist = 0.0; - for (const auto & point : object.shape.footprint.points) { - const float dist = std::hypot(point.x, point.y); - max_dist = max_dist < dist ? dist : max_dist; - } - return max_dist; - } else { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type"); - return std::nullopt; - } -} - } // namespace obstacle_pointcloud_based_validator #include diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/radar_object_clustering/CMakeLists.txt index acb544bfe4f6a..ba911c3035765 100644 --- a/perception/radar_object_clustering/CMakeLists.txt +++ b/perception/radar_object_clustering/CMakeLists.txt @@ -37,4 +37,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/radar_object_clustering/config/radar_object_clustering.param.yaml b/perception/radar_object_clustering/config/radar_object_clustering.param.yaml new file mode 100644 index 0000000000000..2abbf14622a3f --- /dev/null +++ b/perception/radar_object_clustering/config/radar_object_clustering.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + # clustering parameter + angle_threshold: 0.174 # [rad] (10 deg) + distance_threshold: 10.0 # [m] + velocity_threshold: 4.0 # [m/s] + + # output object settings + # set false if you want to use the object information from radar + is_fixed_label: true + fixed_label: "CAR" + is_fixed_size: true + size_x: 4.0 # [m] + size_y: 1.5 # [m] + size_z: 1.5 # [m] diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml index e216bec45798a..68fca44bcc723 100644 --- a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml +++ b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml @@ -4,28 +4,12 @@ - - - - - - - - - + - - - - - - - - - + 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 74e85bde21385..8612fa19ca7f3 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 @@ -71,15 +71,15 @@ RadarObjectClusteringNode::RadarObjectClusteringNode(const rclcpp::NodeOptions & std::bind(&RadarObjectClusteringNode::onSetParam, this, std::placeholders::_1)); // Node Parameter - node_param_.angle_threshold = declare_parameter("angle_threshold", 0.174); - node_param_.distance_threshold = declare_parameter("distance_threshold", 4.0); - node_param_.velocity_threshold = declare_parameter("velocity_threshold", 2.0); - node_param_.is_fixed_label = declare_parameter("is_fixed_label", false); - node_param_.fixed_label = declare_parameter("fixed_label", "UNKNOWN"); - node_param_.is_fixed_size = declare_parameter("is_fixed_size", false); - node_param_.size_x = declare_parameter("size_x", 4.0); - node_param_.size_y = declare_parameter("size_y", 1.5); - node_param_.size_z = declare_parameter("size_z", 1.5); + node_param_.angle_threshold = declare_parameter("angle_threshold"); + node_param_.distance_threshold = declare_parameter("distance_threshold"); + node_param_.velocity_threshold = declare_parameter("velocity_threshold"); + node_param_.is_fixed_label = declare_parameter("is_fixed_label"); + node_param_.fixed_label = declare_parameter("fixed_label"); + node_param_.is_fixed_size = declare_parameter("is_fixed_size"); + node_param_.size_x = declare_parameter("size_x"); + node_param_.size_y = declare_parameter("size_y"); + node_param_.size_z = declare_parameter("size_z"); // Subscriber sub_objects_ = create_subscription( diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 96b6ca452bbcc..ad06a1a9f6a10 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -190,7 +190,8 @@ max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: - prepare_time: 2.0 # [s] + min_prepare_time: 1.0 # [s] + max_prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index f98eac5315006..0cb7f3f1a7a92 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -81,6 +81,7 @@ regulation: crosswalk: false intersection: false + traffic_light: true # ego vehicle stuck detection stuck_detection: 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 4a42da9bc5ac3..3a94a18e011b9 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 @@ -1,6 +1,9 @@ /**: ros__parameters: start_planner: + + verbose: false + th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 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 e4044d7191805..87340cc665edd 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 @@ -68,84 +68,6 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using tier4_autoware_utils::Polygon2d; -#define DEFINE_SETTER(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER(TYPE, NAME) \ - DEFINE_SETTER(TYPE, NAME) \ - DEFINE_GETTER(TYPE, NAME) - -class PullOverStatus -{ -public: - // Reset all data members to their initial states - void reset() - { - lane_parking_pull_over_path_ = nullptr; - current_path_idx_ = 0; - require_increment_ = true; - prev_stop_path_ = nullptr; - prev_stop_path_after_approval_ = nullptr; - current_lanes_.clear(); - pull_over_lanes_.clear(); - lanes_.clear(); - has_decided_path_ = false; - is_safe_dynamic_objects_ = false; - prev_found_path_ = false; - prev_is_safe_dynamic_objects_ = false; - has_decided_velocity_ = false; - } - - DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_SETTER_GETTER(size_t, current_path_idx) - DEFINE_SETTER_GETTER(bool, require_increment) - DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path) - DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path_after_approval) - DEFINE_SETTER_GETTER(lanelet::ConstLanelets, current_lanes) - DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) - DEFINE_SETTER_GETTER(std::vector, lanes) - DEFINE_SETTER_GETTER(bool, has_decided_path) - DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) - DEFINE_SETTER_GETTER(bool, prev_found_path) - DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) - DEFINE_SETTER_GETTER(bool, has_decided_velocity) - DEFINE_SETTER_GETTER(Pose, refined_goal_pose) - DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) - -private: - std::shared_ptr lane_parking_pull_over_path_{nullptr}; - size_t current_path_idx_{0}; - bool require_increment_{true}; - std::shared_ptr prev_stop_path_{nullptr}; - std::shared_ptr prev_stop_path_after_approval_{nullptr}; - lanelet::ConstLanelets current_lanes_{}; - lanelet::ConstLanelets pull_over_lanes_{}; - std::vector lanes_{}; - bool has_decided_path_{false}; - bool is_safe_dynamic_objects_{false}; - bool prev_found_path_{false}; - bool prev_is_safe_dynamic_objects_{false}; - bool has_decided_velocity_{false}; - - Pose refined_goal_pose_{}; - Pose closest_goal_candidate_pose_{}; -}; - -#undef DEFINE_SETTER -#undef DEFINE_GETTER -#undef DEFINE_SETTER_GETTER - #define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ public: \ void set_##NAME(const TYPE & value) \ @@ -177,6 +99,10 @@ class ThreadSafeData bool incrementPathIndex() { const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + if (pull_over_path_->incrementPathIndex()) { last_path_idx_increment_time_ = clock_->now(); return true; @@ -184,20 +110,39 @@ class ThreadSafeData return false; } - void set_pull_over_path(const PullOverPath & value) + void set_pull_over_path(const PullOverPath & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = std::make_shared(value); + pull_over_path_ = std::make_shared(path); + if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = std::make_shared(path); + } + last_path_update_time_ = clock_->now(); } - void set_pull_over_path(const std::shared_ptr & value) + void set_pull_over_path(const std::shared_ptr & path) { const std::lock_guard lock(mutex_); - pull_over_path_ = value; + pull_over_path_ = path; + if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = path; + } last_path_update_time_ = clock_->now(); } + template + void set(Args... args) + { + std::lock_guard lock(mutex_); + (..., set(args)); + } + void set(const GoalCandidates & arg) { set_goal_candidates(arg); } + void set(const std::vector & arg) { set_pull_over_path_candidates(arg); } + void set(const std::shared_ptr & arg) { set_pull_over_path(arg); } + void set(const PullOverPath & arg) { set_pull_over_path(arg); } + void set(const GoalCandidate & arg) { set_modified_goal_pose(arg); } + void clearPullOverPath() { const std::lock_guard lock(mutex_); @@ -237,6 +182,7 @@ class ThreadSafeData } DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) @@ -247,6 +193,7 @@ class ThreadSafeData private: std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; std::vector pull_over_path_candidates_; GoalCandidates goal_candidates_{}; std::optional modified_goal_pose_; @@ -283,6 +230,22 @@ struct LastApprovalData Pose pose{}; }; +struct PreviousPullOverData +{ + void reset() + { + stop_path = nullptr; + stop_path_after_approval = nullptr; + found_path = false; + is_safe = false; + } + + std::shared_ptr stop_path{nullptr}; + std::shared_ptr stop_path_after_approval{nullptr}; + bool found_path{false}; + bool is_safe{false}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -303,27 +266,47 @@ class GoalPlannerModule : public SceneModuleInterface } } - CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; bool isExecutionRequested() const override; bool isExecutionReady() const override; void processOnExit() override; void updateData() override; + void postProcess() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } + CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: + /*　 + * state transitions and plan function used in each state + * + * +--------------------------+ + * | RUNNING | + * | planPullOverAsCandidate()| + * +------------+-------------+ + * | hasDecidedPath() + * 2 v + * +--------------------------+ + * | WAITING_APPROVAL | + * | planPullOverAsCandidate()| + * +------------+-------------+ + * | isActivated() + * 3 v + * +--------------------------+ + * | RUNNING | + * | planPullOverAsOutput() | + * +--------------------------+ + */ + // The start_planner activates when it receives a new route, // so there is no need to terminate the goal planner. // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } - bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; @@ -355,10 +338,10 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; std::recursive_mutex mutex_; - PullOverStatus status_; ThreadSafeData thread_safe_data_; std::unique_ptr last_approval_data_{nullptr}; + PreviousPullOverData prev_data_{nullptr}; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -383,20 +366,24 @@ class GoalPlannerModule : public SceneModuleInterface // collision check void initializeOccupancyGridMap(); void updateOccupancyGrid(); - bool checkCollision(const PathWithLaneId & path) const; + bool checkOccupancyGridCollision(const PathWithLaneId & path) const; + bool checkObjectsCollision( + const PathWithLaneId & path, const double collision_check_margin, + const bool update_debug_data = false) const; // goal seach Pose calcRefinedGoal(const Pose & goal_pose) const; - void generateGoalCandidates(); + GoalCandidates generateGoalCandidates() const; // stop or decelerate + void deceleratePath(PullOverPath & pull_over_path) const; void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; void decelerateBeforeSearchStart( const Pose & search_start_offset_pose, PathWithLaneId & path) const; - PathWithLaneId generateStopPath(); - PathWithLaneId generateFeasibleStopPath(); + PathWithLaneId generateStopPath() const; + PathWithLaneId generateFeasibleStopPath() const; - void keepStoppedWithCurrentPath(PathWithLaneId & path); + void keepStoppedWithCurrentPath(PathWithLaneId & path) const; double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; // status @@ -411,6 +398,7 @@ class GoalPlannerModule : public SceneModuleInterface bool hasDecidedPath() const; void decideVelocity(); bool foundPullOverPath() const; + void updateStatus(const BehaviorModuleOutput & output); // validation bool hasEnoughDistance(const PullOverPath & pull_over_path) const; @@ -423,26 +411,27 @@ class GoalPlannerModule : public SceneModuleInterface // freespace parking bool planFreespacePath(); - void returnToLaneParking(); + bool canReturnToLaneParking(); // plan pull over path - BehaviorModuleOutput planWithGoalModification(); - BehaviorModuleOutput planWaitingApprovalWithGoalModification(); - void selectSafePullOverPath(); + BehaviorModuleOutput planPullOver(); + BehaviorModuleOutput planPullOverAsOutput(); + BehaviorModuleOutput planPullOverAsCandidate(); + std::optional> selectPullOverPath( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const double collision_check_margin) const; std::vector sortPullOverPathCandidatesByGoalPriority( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; - // deal with pull over partial paths - void transitionToNextPathIfFinishingCurrentPath(); - // lanes and drivable area - void setLanes(); + std::vector generateDrivableLanes() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // output setter - void setOutput(BehaviorModuleOutput & output); - void setStopPath(BehaviorModuleOutput & output); + void setOutput(BehaviorModuleOutput & output) const; + void setStopPath(BehaviorModuleOutput & output) const; + void updatePreviousData(const BehaviorModuleOutput & output); /** * @brief Sets a stop path in the current path based on safety conditions and previous paths. @@ -453,7 +442,7 @@ class GoalPlannerModule : public SceneModuleInterface * * @param output BehaviorModuleOutput */ - void setStopPathFromCurrentPath(BehaviorModuleOutput & output); + void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const; void setModifiedGoal(BehaviorModuleOutput & output) const; void setTurnSignalInfo(BehaviorModuleOutput & output) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 503542be7cda6..4a117e5350baf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -135,6 +135,9 @@ class NormalLaneChange : public LaneChangeBase bool hasEnoughLengthToIntersection( const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index 7c04cab5c081f..fce31b6db7369 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -49,8 +49,6 @@ class SideShiftModule : public SceneModuleInterface bool isExecutionReady() const override; bool isReadyForNextRequest( const double & min_request_time_sec, bool override_requests = false) const noexcept; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; void updateData() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -70,32 +68,12 @@ class SideShiftModule : public SceneModuleInterface { } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override - { - updateData(); - - if (!isWaitingApproval()) { - return plan(); - } - - // module is waiting approval. Check it. - if (isActivated()) { - RCLCPP_DEBUG(getLogger(), "Was waiting approval, and now approved. Do plan()."); - return plan(); - } else { - RCLCPP_DEBUG(getLogger(), "keep waiting approval... Do planCandidate()."); - return planWaitingApproval(); - } - } - private: - bool canTransitSuccessState() override { return false; } + bool canTransitSuccessState() override; bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } - rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; + bool canTransitIdleToRunningState() override { return true; } void initVariables(); 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 8d2532504926e..54e13de85e8da 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 @@ -91,8 +91,6 @@ class StartPlannerModule : public SceneModuleInterface bool isExecutionRequested() const override; bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] void updateCurrentState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; CandidateOutput planCandidate() const override; @@ -123,14 +121,16 @@ class StartPlannerModule : public SceneModuleInterface bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: - bool canTransitSuccessState() override { return false; } + bool canTransitSuccessState() override; bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + bool canTransitIdleToRunningState() override; void initializeSafetyCheckParameters(); + bool receivedNewRoute() const; + bool isModuleRunning() const; bool isCurrentPoseOnMiddleOfTheRoad() const; bool isCloseToOriginalStartPose() const; @@ -209,7 +209,7 @@ class StartPlannerModule : public SceneModuleInterface void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // check if the goal is located behind the ego in the same route segment. - bool IsGoalBehindOfEgoInSameRouteSegment() const; + bool isGoalBehindOfEgoInSameRouteSegment() const; // generate BehaviorPathOutput with stopping path and update status BehaviorModuleOutput generateStopOutput(); @@ -220,6 +220,7 @@ class StartPlannerModule : public SceneModuleInterface bool planFreespacePath(); void setDebugData() const; + void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index c8b87c7b4759f..60cf9b1244236 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -211,7 +211,8 @@ struct AvoidanceParameters double stop_buffer{0.0}; // start avoidance after this time to avoid sudden path change - double prepare_time{0.0}; + double min_prepare_time{0.0}; + double max_prepare_time{0.0}; // Even if the vehicle speed is zero, avoidance will start after a distance of this much. double min_prepare_distance{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 11388dd6bb74a..e1d51122e5c1b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -91,10 +91,16 @@ class AvoidanceHelper return std::max(getEgoSpeed(), values.at(idx)); } + double getMinimumPrepareDistance() const + { + const auto & p = parameters_; + return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance); + } + double getNominalPrepareDistance() const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); + return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); } double getNominalAvoidanceDistance(const double shift_length) const @@ -185,7 +191,8 @@ class AvoidanceHelper max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); return std::clamp( - 1.5 * dynamic_distance, parameters_->object_check_min_forward_distance, + 1.5 * dynamic_distance + getNominalPrepareDistance(), + parameters_->object_check_min_forward_distance, parameters_->object_check_max_forward_distance); } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 4267261fdfe84..5023401263b8d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -53,6 +53,8 @@ struct PullOverPath Pose end_pose{}; std::vector debug_poses{}; size_t goal_id{}; + size_t id{}; + bool decided_velocity{false}; PathWithLaneId getFullPath() const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index c664ae3e15aef..3f53f4b9c51e8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -65,6 +65,7 @@ struct LaneChangeParameters // regulatory elements bool regulate_on_crosswalk{false}; bool regulate_on_intersection{false}; + bool regulate_on_traffic_light{false}; // ego vehicle stuck detection double stop_velocity_threshold{0.1}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 6978a7d494f30..bd4017e5dff7d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -46,6 +46,7 @@ struct StartPlannerParameters double collision_check_distance_from_end{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; + bool verbose{false}; // shift pull out bool enable_shift_pull_out{false}; 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 3f9591f6c2143..4d34c548e640e 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 @@ -45,6 +45,7 @@ #include #include #include +#include #include namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 031926f2d9d1d..f1f681fed06a0 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include #include #include @@ -45,6 +46,7 @@ CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; debug.current_obj_pose = obj.initial_pose.pose; + debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); debug.current_twist = obj.initial_twist.twist; return {tier4_autoware_utils::toHexString(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 2ad0928696a5c..ea4eab425b66b 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -222,6 +222,19 @@ bool AvoidanceModule::canTransitSuccessState() const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; + // If the vehicle is on the shift line, keep RUNNING. + { + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + const auto within = [](const auto & line, const size_t idx) { + return line.start_idx < idx && idx < line.end_idx; + }; + for (const auto & shift_line : path_shifter_.getShiftLines()) { + if (within(shift_line, idx)) { + return false; + } + } + } + // Nothing to do. -> EXIT. if (!has_avoidance_target) { if (!has_shift_point && !has_base_offset) { @@ -424,10 +437,27 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_.isShifted()) { + RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated."); return false; } + // prevent sudden steering. + const auto registered_lines = path_shifter_.getShiftLines(); + if (!registered_lines.empty()) { + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + const auto to_shift_start_point = calcSignedArcLength( + path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); + if (to_shift_start_point < helper_.getMinimumPrepareDistance()) { + RCLCPP_DEBUG( + getLogger(), + "Distance to shift start point is less than minimum prepare distance. The distance is not " + "enough."); + return false; + } + } + if (!data.stop_target_object) { + RCLCPP_DEBUG(getLogger(), "can pass by the object safely without avoidance maneuver."); return true; } @@ -907,8 +937,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( const auto & base_link2rear = planner_data_->parameters.base_link2rear; // Calculate feasible shift length - const auto get_shift_length = - [&](auto & object, const auto & desire_shift_length) -> boost::optional { + const auto get_shift_profile = + [&]( + auto & object, const auto & desire_shift_length) -> std::optional> { // use each object param const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -918,55 +949,57 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( const auto avoiding_shift = desire_shift_length - current_ego_shift; const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift); + // calculate remaining distance. + const auto prepare_distance = helper_.getNominalPrepareDistance(); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; + const auto constant = object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal + prepare_distance; + const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; + const auto remaining_distance = object.longitudinal - constant; + const auto avoidance_distance = + has_enough_distance ? nominal_avoid_distance : remaining_distance; + + // nominal case. avoidable. + if (has_enough_distance) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + if (!isBestEffort(parameters_->policy_lateral_margin)) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } // ego already has enough positive shift. const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3; if (is_object_on_right && has_enough_positive_shift) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } // ego already has enough negative shift. const auto has_enough_negative_shift = avoiding_shift > 1e-3 && desire_shift_length < -1e-3; if (!is_object_on_right && has_enough_negative_shift) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } // don't relax shift length since it can stop in front of the object. if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } - // calculate remaining distance. - const auto prepare_distance = helper_.getNominalPrepareDistance(); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto constant = object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + prepare_distance; - const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; - const auto remaining_distance = object.longitudinal - constant; - // the avoidance path is already approved const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; const auto is_approved = (helper_.getShift(object_pos) > 0.0 && is_object_on_right) || (helper_.getShift(object_pos) < 0.0 && !is_object_on_right); if (is_approved) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } // prepare distance is not enough. unavoidable. if (remaining_distance < 1e-3) { object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; - return boost::none; - } - - // nominal case. avoidable. - if (has_enough_distance) { - return desire_shift_length; + return std::nullopt; } // calculate lateral jerk. @@ -975,13 +1008,13 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // relax lateral jerk limit. avoidable. if (required_jerk < helper_.getLateralMaxJerkLimit()) { - return desire_shift_length; + return std::make_pair(desire_shift_length, avoidance_distance); } // avoidance distance is not enough. unavoidable. if (!isBestEffort(parameters_->policy_deceleration)) { object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return boost::none; + return std::nullopt; } // output avoidance path under lateral jerk constraints. @@ -990,21 +1023,21 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { object.reason = "LessThanExecutionThreshold"; - return boost::none; + return std::nullopt; } const auto feasible_shift_length = desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift : -1.0 * feasible_relative_shift_length + current_ego_shift; - const auto feasible = + const auto infeasible = std::abs(feasible_shift_length - object.overhang_dist) < 0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; - if (feasible) { + if (infeasible) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. "); - object.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; - return boost::none; + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return std::nullopt; } { @@ -1013,7 +1046,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( std::abs(avoiding_shift), feasible_relative_shift_length); } - return feasible_shift_length; + return std::make_pair(feasible_shift_length, avoidance_distance); }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; @@ -1048,9 +1081,9 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // use each object param const auto object_type = utils::getHighestProbLabel(o.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto feasible_shift_length = get_shift_length(o, desire_shift_length); + const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - if (!feasible_shift_length) { + if (!feasible_shift_profile.has_value()) { if (o.avoid_required && is_forward_object(o)) { break; } else { @@ -1059,10 +1092,8 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( } // use absolute dist for return-to-center, relative dist from current for avoiding. - const auto feasible_avoid_distance = - helper_.getMaxAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); const auto feasible_return_distance = - helper_.getMaxAvoidanceDistance(feasible_shift_length.get()); + helper_.getMaxAvoidanceDistance(feasible_shift_profile.value().first); AvoidLine al_avoid; { @@ -1078,14 +1109,15 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // start point (use previous linear shift length as start shift length.) al_avoid.start_longitudinal = [&]() { - const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3); + const auto nearest_avoid_distance = + std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3); if (data.to_start_point > to_shift_end) { return nearest_avoid_distance; } const auto minimum_avoid_distance = - helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); + helper_.getMinAvoidanceDistance(feasible_shift_profile.value().first - current_ego_shift); const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3); return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); @@ -1097,7 +1129,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); // end point - al_avoid.end_shift_length = feasible_shift_length.get(); + al_avoid.end_shift_length = feasible_shift_profile.value().first; al_avoid.end_longitudinal = to_shift_end; // misc @@ -1112,7 +1144,7 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( const auto to_shift_start = o.longitudinal + offset; // start point - al_return.start_shift_length = feasible_shift_length.get(); + al_return.start_shift_length = feasible_shift_profile.value().first; al_return.start_longitudinal = to_shift_start; // end point @@ -1639,6 +1671,10 @@ void AvoidanceModule::applySmallShiftFilter( continue; } + if (s.start_longitudinal < helper_.getMinimumPrepareDistance()) { + continue; + } + shift_lines.push_back(s); } } @@ -2025,13 +2061,16 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); - const size_t prev_ego_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + const auto prev_ego_idx = motion_utils::findNearestSegmentIndex( previous_path.points, getPose(original_path.points.at(orig_ego_idx)), std::numeric_limits::max(), planner_data_->parameters.ego_nearest_yaw_threshold); + if (!prev_ego_idx) { + return original_path; + } size_t clip_idx = 0; for (size_t i = 0; i < prev_ego_idx; ++i) { - if (backward_length > calcSignedArcLength(previous_path.points, clip_idx, prev_ego_idx)) { + if (backward_length > calcSignedArcLength(previous_path.points, clip_idx, *prev_ego_idx)) { break; } clip_idx = i; @@ -2041,7 +2080,7 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig { extended_path.points.insert( extended_path.points.end(), previous_path.points.begin() + clip_idx, - previous_path.points.begin() + prev_ego_idx); + previous_path.points.begin() + *prev_ego_idx); } // overwrite backward path velocity by latest one. @@ -2389,9 +2428,8 @@ AvoidLineArray AvoidanceModule::findNewShiftLine( for (size_t i = 0; i < shift_lines.size(); ++i) { const auto & candidate = shift_lines.at(i); - // new shift points must exist in front of Ego - // this value should be larger than -eps consider path shifter calculation error. - if (candidate.start_idx < avoid_data_.ego_closest_path_index) { + // prevent sudden steering. + if (candidate.start_longitudinal < helper_.getMinimumPrepareDistance()) { break; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 6ed67fc817f28..f71024dcc5034 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -221,7 +221,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( // avoidance maneuver (longitudinal) { std::string ns = "avoidance.avoidance.longitudinal."; - p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); @@ -390,7 +391,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "prepare_time", p->prepare_time); + updateParam(parameters, ns + "min_prepare_time", p->min_prepare_time); + updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); } diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 0475cd92f1bec..6fdb46d058aef 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 @@ -54,7 +54,7 @@ namespace behavior_path_planner GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map) + const std::unordered_map> & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, @@ -121,7 +121,7 @@ GoalPlannerModule::GoalPlannerModule( goal_planner_data_.collision_check); } - status_.reset(); + prev_data_.reset(); } // This function is needed for waiting for planner_data_ @@ -174,6 +174,7 @@ void GoalPlannerModule::onTimer() auto pull_over_path = planner->plan(goal_candidate.goal_pose); if (pull_over_path && isCrossingPossible(*pull_over_path)) { pull_over_path->goal_id = goal_candidate.id; + pull_over_path->id = path_candidates.size(); path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = @@ -240,7 +241,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() void GoalPlannerModule::updateData() { - if (getCurrentStatus() == ModuleStatus::IDLE) { + if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { return; } @@ -253,12 +254,33 @@ void GoalPlannerModule::updateData() initializeOccupancyGridMap(); } + resetPathCandidate(); + resetPathReference(); + path_reference_ = getPreviousModuleOutput().reference_path; + updateOccupancyGrid(); - // set current road lanes, pull over lanes, and drivable lane - setLanes(); + // update goal searcher and generate goal candidates + if (thread_safe_data_.get_goal_candidates().empty()) { + goal_searcher_->setPlannerData(planner_data_); + goal_searcher_->setReferenceGoal( + calcRefinedGoal(planner_data_->route_handler->getOriginalGoalPose())); + thread_safe_data_.set_goal_candidates(generateGoalCandidates()); + } + + if (!isActivated()) { + return; + } - generateGoalCandidates(); + if (hasFinishedCurrentPath()) { + thread_safe_data_.incrementPathIndex(); + } + + if (!last_approval_data_) { + last_approval_data_ = + std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); + decideVelocity(); + } } void GoalPlannerModule::initializeOccupancyGridMap() @@ -290,7 +312,7 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - status_.reset(); + prev_data_.reset(); last_approval_data_.reset(); } @@ -500,20 +522,28 @@ bool GoalPlannerModule::planFreespacePath() return false; } -void GoalPlannerModule::returnToLaneParking() +bool GoalPlannerModule::canReturnToLaneParking() { // return only before starting free space parking if (!isStopped()) { - return; + return false; } - if (!status_.get_lane_parking_pull_over_path()) { - return; + if (!thread_safe_data_.get_lane_parking_pull_over_path()) { + return false; } - const PathWithLaneId path = status_.get_lane_parking_pull_over_path()->getFullPath(); - if (checkCollision(path)) { - return; + const PathWithLaneId path = thread_safe_data_.get_lane_parking_pull_over_path()->getFullPath(); + + if ( + parameters_->use_object_recognition && + checkObjectsCollision(path, parameters_->object_recognition_collision_check_margin)) { + return false; + } + + if ( + parameters_->use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(path)) { + return false; } const Point & current_point = planner_data_->self_odometry->pose.pose.position; @@ -521,64 +551,40 @@ void GoalPlannerModule::returnToLaneParking() const bool is_close_to_path = std::abs(motion_utils::calcLateralOffset(path.points, current_point)) < th_distance; if (!is_close_to_path) { - return; + return false; } - status_.set_has_decided_path(false); - thread_safe_data_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); - - RCLCPP_INFO(getLogger(), "return to lane parking"); + return true; } -void GoalPlannerModule::generateGoalCandidates() +GoalCandidates GoalPlannerModule::generateGoalCandidates() const { - const auto & route_handler = planner_data_->route_handler; - - // with old architecture, module instance is not cleared when new route is received - // so need to reset status here. - // todo: move this check out of this function after old architecture is removed - if (!thread_safe_data_.get_goal_candidates().empty()) { - return; - } - // calculate goal candidates - const Pose goal_pose = route_handler->getOriginalGoalPose(); - status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); + const auto & route_handler = planner_data_->route_handler; if (goal_planner_utils::isAllowedGoalModification(route_handler)) { - const std::lock_guard lock(mutex_); - goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); - thread_safe_data_.set_goal_candidates(goal_searcher_->search()); - const auto current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, false); - status_.set_closest_goal_candidate_pose( - goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()) - .goal_pose); - } else { - GoalCandidate goal_candidate{}; - goal_candidate.goal_pose = goal_pose; - goal_candidate.distance_from_original_goal = 0.0; - GoalCandidates goal_candidates{}; - goal_candidates.push_back(goal_candidate); - thread_safe_data_.set_goal_candidates(goal_candidates); - status_.set_closest_goal_candidate_pose(goal_pose); + return goal_searcher_->search(); } + + // NOTE: + // currently since pull over is performed only when isAllowedGoalModification is true, + // never be in the following process. + GoalCandidate goal_candidate{}; + goal_candidate.goal_pose = route_handler->getOriginalGoalPose(); + goal_candidate.distance_from_original_goal = 0.0; + GoalCandidates goal_candidates{}; + goal_candidates.push_back(goal_candidate); + + return goal_candidates; } BehaviorModuleOutput GoalPlannerModule::plan() { - resetPathCandidate(); - resetPathReference(); - - path_reference_ = getPreviousModuleOutput().reference_path; - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planWithGoalModification(); - } else { - fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); - return fixed_goal_planner_->plan(planner_data_); + return planPullOver(); } + + fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); + return fixed_goal_planner_->plan(planner_data_); } std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( @@ -600,6 +606,48 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; }); + if (parameters_->use_object_recognition) { + // Create a map of PullOverPath pointer to largest collision check margin + auto calcLargestMargin = [&](const PullOverPath & pull_over_path) { + // check has safe goal + const auto goal_candidate_it = std::find_if( + goal_candidates.begin(), goal_candidates.end(), + [pull_over_path](const auto & goal_candidate) { + return goal_candidate.id == pull_over_path.goal_id; + }); + if (goal_candidate_it != goal_candidates.end() && !goal_candidate_it->is_safe) { + return 0.0; + } + // calc largest margin + std::vector scale_factors{3.0, 2.0, 1.0}; + const double margin = parameters_->object_recognition_collision_check_margin; + const auto resampled_path = + utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); + for (const auto & scale_factor : scale_factors) { + if (!checkObjectsCollision(resampled_path, margin * scale_factor)) { + return margin * scale_factor; + } + } + return 0.0; + }; + + // Create a map of PullOverPath pointer to largest collision check margin + std::map path_id_to_margin_map; + for (const auto & path : sorted_pull_over_path_candidates) { + path_id_to_margin_map[path.id] = calcLargestMargin(path); + } + + // sorts in descending order so the item with larger margin comes first + std::stable_sort( + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), + [&path_id_to_margin_map](const PullOverPath & a, const PullOverPath & b) { + if (std::abs(path_id_to_margin_map[a.id] - path_id_to_margin_map[b.id]) < 0.01) { + return false; + } + return path_id_to_margin_map[a.id] > path_id_to_margin_map[b.id]; + }); + } + // Sort pull_over_path_candidates based on the order in efficient_path_order if (parameters_->path_priority == "efficient_path") { std::stable_sort( @@ -615,23 +663,10 @@ std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPri return sorted_pull_over_path_candidates; } -void GoalPlannerModule::selectSafePullOverPath() +std::optional> GoalPlannerModule::selectPullOverPath( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const double collision_check_margin) const { - // select safe lane pull over path from candidates - std::vector pull_over_path_candidates{}; - GoalCandidates goal_candidates{}; - { - const std::lock_guard lock(mutex_); - goal_searcher_->setPlannerData(planner_data_); - goal_candidates = thread_safe_data_.get_goal_candidates(); - goal_searcher_->update(goal_candidates); - thread_safe_data_.set_goal_candidates(goal_candidates); - thread_safe_data_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( - thread_safe_data_.get_pull_over_path_candidates(), thread_safe_data_.get_goal_candidates())); - pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates(); - thread_safe_data_.clearPullOverPath(); - } - for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -643,74 +678,53 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - // check if path is valid and safe + if (!hasEnoughDistance(pull_over_path)) { + continue; + } + + const auto resampled_path = utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5); if ( - !hasEnoughDistance(pull_over_path) || - checkCollision(utils::resamplePathWithSpline(pull_over_path.getParkingPath(), 0.5))) { + parameters_->use_object_recognition && + checkObjectsCollision(resampled_path, collision_check_margin, true /*update_debug_data*/)) { continue; } - // found safe pull over path - { - const std::lock_guard lock(mutex_); - thread_safe_data_.set_pull_over_path(pull_over_path); - thread_safe_data_.set_modified_goal_pose(*goal_candidate_it); - status_.set_lane_parking_pull_over_path(thread_safe_data_.get_pull_over_path()); + if ( + parameters_->use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(resampled_path)) { + continue; } - break; - } - if (!thread_safe_data_.foundPullOverPath()) { - return; + return std::make_pair(pull_over_path, *goal_candidate_it); } - // decelerate before the search area start - const auto search_start_offset_pose = calcLongitudinalOffsetPose( - thread_safe_data_.get_pull_over_path()->getFullPath().points, - status_.get_refined_goal_pose().position, - -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - - approximate_pull_over_distance_); - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); - if (search_start_offset_pose) { - decelerateBeforeSearchStart(*search_start_offset_pose, first_path); - } else { - // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - parameters_->pull_over_velocity); - for (auto & p : first_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); - if (min_decel_distance && distance_from_ego < *min_decel_distance) { - continue; - } - p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); - } - } + return {}; } -void GoalPlannerModule::setLanes() +std::vector GoalPlannerModule::generateDrivableLanes() const { - const std::lock_guard lock(mutex_); - status_.set_current_lanes(utils::getExtendedCurrentLanes( + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false)); - status_.set_pull_over_lanes(goal_planner_utils::getPullOverLanes( + /*forward_only_in_route*/ false); + const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length)); - status_.set_lanes(utils::generateDrivableLanesWithShoulderLanes( - status_.get_current_lanes(), status_.get_pull_over_lanes())); + parameters_->forward_goal_search_length); + return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } -void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) +void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const { + output.reference_path = getPreviousModuleOutput().reference_path; + if (!thread_safe_data_.foundPullOverPath()) { // situation : not safe against static objects use stop_path setStopPath(output); - } else if ( - parameters_->safety_check_params.enable_safety_check && !isSafePath() && - status_.get_has_decided_path() && isActivated()) { + setDrivableAreaInfo(output); + return; + } + + if (parameters_->safety_check_params.enable_safety_check && !isSafePath() && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -718,62 +732,43 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) } else { // situation : (safe against static and dynamic objects) or (safe against static objects and // before approval) don't stop - if (isActivated()) { - resetWallPoses(); - } // keep stop if not enough time passed, // because it takes time for the trajectory to be reflected auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); keepStoppedWithCurrentPath(current_path); - output.path = std::make_shared(current_path); - output.reference_path = getPreviousModuleOutput().reference_path; } - setDrivableAreaInfo(output); - setModifiedGoal(output); + setDrivableAreaInfo(output); // set hazard and turn signal - if (status_.get_has_decided_path()) { + if (hasDecidedPath() && isActivated()) { setTurnSignalInfo(output); } - - // for the next loop setOutput(). - // this is used to determine whether to generate a new stop path or keep the current stop path. - const std::lock_guard lock(mutex_); - status_.set_prev_found_path(thread_safe_data_.foundPullOverPath()); - status_.set_prev_is_safe_dynamic_objects( - parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); } -void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) +void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const { - if (status_.get_prev_found_path() || !status_.get_prev_stop_path()) { + if (prev_data_.found_path || !prev_data_.stop_path) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - const std::lock_guard lock(mutex_); - status_.set_prev_stop_path(output.path); - // set stop path as pull over path - auto stop_pull_over_path = std::make_shared(); - stop_pull_over_path->partial_paths.push_back(*output.path); - thread_safe_data_.set_pull_over_path(stop_pull_over_path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path - output.path = status_.get_prev_stop_path(); + output.path = prev_data_.stop_path; + // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } - output.reference_path = getPreviousModuleOutput().reference_path; } -void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) +void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (status_.get_prev_is_safe_dynamic_objects() || !status_.get_prev_stop_path_after_approval()) { + if (prev_data_.is_safe || !prev_data_.stop_path_after_approval) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -781,7 +776,6 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = std::make_shared(*stop_path); - status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { output.path = @@ -790,14 +784,13 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } - // status_.set_last_path_update_time(std::make_shared(clock_->now())); } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = status_.get_prev_stop_path_after_approval(); + output.path = prev_data_.stop_path_after_approval; + // stop_pose_ is removed in manager every loop, so need to set every loop. stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } - output.reference_path = getPreviousModuleOutput().reference_path; } void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const @@ -808,7 +801,7 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *output.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); + *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -860,13 +853,10 @@ void GoalPlannerModule::updateSteeringFactor( pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, ""); } +// NOTE: Once this function returns true, it will continue to return true thereafter. Because +// selectPullOverPath() will not select new path. bool GoalPlannerModule::hasDecidedPath() const { - // once decided, keep the decision - if (status_.get_has_decided_path()) { - return true; - } - // if path is not safe, not decided if (!thread_safe_data_.foundPullOverPath()) { return false; @@ -874,18 +864,15 @@ bool GoalPlannerModule::hasDecidedPath() const // if ego is sufficiently close to the start of the nearest candidate path, the path is decided const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( - thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose, - std::numeric_limits::max(), M_PI_2); - if (!ego_segment_idx) { - return false; - } + const size_t ego_segment_idx = motion_utils::findNearestSegmentIndex( + thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position); + const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( thread_safe_data_.get_pull_over_path()->getCurrentPath().points, thread_safe_data_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( thread_safe_data_.get_pull_over_path()->getCurrentPath().points, current_pose.position, - *ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, + ego_segment_idx, thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); return dist_to_parking_start_pose < parameters_->decide_path_distance; } @@ -894,67 +881,98 @@ void GoalPlannerModule::decideVelocity() { const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - // decide velocity to guarantee turn signal lighting time - if (!status_.get_has_decided_velocity()) { - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); - const auto vel = - static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); - for (auto & p : first_path.points) { - p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); - } + auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); + const auto vel = + static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); + for (auto & p : first_path.points) { + p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } - status_.set_has_decided_velocity(true); } -CandidateOutput GoalPlannerModule::planCandidate() const +BehaviorModuleOutput GoalPlannerModule::planPullOver() { - return CandidateOutput( - thread_safe_data_.get_pull_over_path() ? thread_safe_data_.get_pull_over_path()->getFullPath() - : PathWithLaneId()); + if (!hasDecidedPath()) { + return planPullOverAsCandidate(); + } + + return planPullOverAsOutput(); } -BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() +BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate() { // if pull over path candidates generation is not finished, use previous module output if (thread_safe_data_.get_pull_over_path_candidates().empty()) { return getPreviousModuleOutput(); } - constexpr double path_update_duration = 1.0; + BehaviorModuleOutput output{}; + const BehaviorModuleOutput pull_over_output = planPullOverAsOutput(); + output.modified_goal = pull_over_output.modified_goal; + output.path = std::make_shared(generateStopPath()); - resetPathCandidate(); - resetPathReference(); + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + *output.path, generateDrivableLanes(), planner_data_->drivable_area_expansion_parameters); - // Check if it needs to decide path - status_.set_has_decided_path(hasDecidedPath()); + DrivableAreaInfo current_drivable_area_info{}; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - // Use decided path - if (status_.get_has_decided_path()) { - if (isActivated() && !last_approval_data_) { - last_approval_data_ = - std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); - decideVelocity(); - } - transitionToNextPathIfFinishingCurrentPath(); - } else if ( - !thread_safe_data_.get_pull_over_path_candidates().empty() && - needPathUpdate(path_update_duration)) { + if (!thread_safe_data_.foundPullOverPath()) { + return output; + } + + return output; +} + +BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput() +{ + // if pull over path candidates generation is not finished, use previous module output + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + return getPreviousModuleOutput(); + } + + if (!hasDecidedPath() && needPathUpdate(1.0 /*path_update_duration*/)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates - // and set it to thread_safe_data_.get_pull_over_path() - selectSafePullOverPath(); + // and set it to thread_safe_data_ + + thread_safe_data_.clearPullOverPath(); + + // update goal candidates + goal_searcher_->setPlannerData(planner_data_); + auto goal_candidates = thread_safe_data_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + + // update pull over path candidates + const auto pull_over_path_candidates = sortPullOverPathCandidatesByGoalPriority( + thread_safe_data_.get_pull_over_path_candidates(), goal_candidates); + + // select pull over path which is safe against static objects and get it's goal + const auto path_and_goal_opt = selectPullOverPath( + pull_over_path_candidates, goal_candidates, + parameters_->object_recognition_collision_check_margin); + + // update thread_safe_data_ + if (path_and_goal_opt) { + auto [pull_over_path, modified_goal] = *path_and_goal_opt; + deceleratePath(pull_over_path); + thread_safe_data_.set( + goal_candidates, pull_over_path_candidates, pull_over_path, modified_goal); + } else { + thread_safe_data_.set(goal_candidates, pull_over_path_candidates); + } } - // else: stop path is generated and set by setOutput() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { - returnToLaneParking(); + if ( + thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && + canReturnToLaneParking()) { + thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } // For debug @@ -968,81 +986,73 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() return output; } - const auto distance_to_path_change = calcDistanceToPathChange(); - if (status_.get_has_decided_path()) { - updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); - } - // TODO(tkhmy) add handle status TRYING - updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose, - thread_safe_data_.get_modified_goal_pose()->goal_pose}, - {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::TURNING); + path_candidate_ = + std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); + updatePreviousData(output); return output; } -BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() +void GoalPlannerModule::postProcess() { - resetPathCandidate(); - resetPathReference(); + if (!thread_safe_data_.foundPullOverPath()) { + return; + } - path_reference_ = getPreviousModuleOutput().reference_path; + const bool has_decided_path = hasDecidedPath(); + const auto distance_to_path_change = calcDistanceToPathChange(); - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { - return planWaitingApprovalWithGoalModification(); - } else { - fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); - return fixed_goal_planner_->plan(planner_data_); + if (has_decided_path) { + updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } + + updateSteeringFactor( + {thread_safe_data_.get_pull_over_path()->start_pose, + thread_safe_data_.get_modified_goal_pose()->goal_pose}, + {distance_to_path_change.first, distance_to_path_change.second}, + has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); + + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); } -BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification() +void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) { - // if pull over path candidates generation is not finished, use previous module output - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { - return getPreviousModuleOutput(); + if (prev_data_.found_path || !prev_data_.stop_path) { + prev_data_.stop_path = output.path; } - BehaviorModuleOutput out; - out.modified_goal = planWithGoalModification().modified_goal; // update status_ - out.path = std::make_shared(generateStopPath()); - out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = std::make_shared(planCandidate().path_candidate); - path_reference_ = getPreviousModuleOutput().reference_path; - - // generate drivable area info for new architecture - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { - const double drivable_area_margin = planner_data_->parameters.vehicle_width; - out.drivable_area_info.drivable_margin = - planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; - } else { - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *out.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); + // for the next loop setOutput(). + // this is used to determine whether to generate a new stop path or keep the current stop path. + prev_data_.found_path = thread_safe_data_.foundPullOverPath(); + prev_data_.is_safe = parameters_->safety_check_params.enable_safety_check ? isSafePath() : true; - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - out.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + if (!isActivated()) { + return; } - if (!thread_safe_data_.foundPullOverPath()) { - return out; + if ( + !parameters_->safety_check_params.enable_safety_check || isSafePath() || + (!prev_data_.is_safe && prev_data_.stop_path_after_approval)) { + return; } - - const auto distance_to_path_change = calcDistanceToPathChange(); - if (status_.get_has_decided_path()) { - updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); + prev_data_.is_safe = false; + const bool is_stop_path = std::any_of( + output.path->points.begin(), output.path->points.end(), + [](const auto & point) { return point.point.longitudinal_velocity_mps == 0.0; }); + if (is_stop_path) { + prev_data_.stop_path_after_approval = output.path; } - updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose, - thread_safe_data_.get_modified_goal_pose()->goal_pose}, - {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); +} - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); +BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() +{ + if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return planPullOverAsCandidate(); + } - return out; + fixed_goal_planner_->setPreviousModuleOutput(getPreviousModuleOutput()); + return fixed_goal_planner_->plan(planner_data_); } std::pair GoalPlannerModule::calcDistanceToPathChange() const @@ -1079,7 +1089,7 @@ void GoalPlannerModule::setParameters(const std::shared_ptrroute_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -1087,17 +1097,29 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - if (status_.get_current_lanes().empty()) { + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); + + if (current_lanes.empty()) { return PathWithLaneId{}; } // generate reference path - const auto s_current = - lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; - auto reference_path = - route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); + auto reference_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + + // calculate search start offset pose from the closest goal candidate pose with + // approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible + // stop point is found, stop at this position. + const auto closest_goal_candidate = + goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); + const auto decel_pose = calcLongitudinalOffsetPose( + reference_path.points, closest_goal_candidate.goal_pose.position, + -approximate_pull_over_distance_); // if not approved stop road lane. // stop point priority is @@ -1107,27 +1129,24 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto search_start_offset_pose = calcLongitudinalOffsetPose( - reference_path.points, status_.get_closest_goal_candidate_pose().position, - -approximate_pull_over_distance_); - if ( - !thread_safe_data_.foundPullOverPath() && !thread_safe_data_.get_closest_start_pose() && - !search_start_offset_pose) { - return generateFeasibleStopPath(); - } - - const Pose stop_pose = [&]() -> Pose { + const auto stop_pose = std::invoke([&]() -> boost::optional { if (thread_safe_data_.foundPullOverPath()) { return thread_safe_data_.get_pull_over_path()->start_pose; } if (thread_safe_data_.get_closest_start_pose()) { return thread_safe_data_.get_closest_start_pose().value(); } - return *search_start_offset_pose; - }(); + if (!decel_pose) { + return boost::optional{}; + } + return decel_pose.value(); + }); + if (!stop_pose) { + return generateFeasibleStopPath(); + } // if stop pose is closer than min_stop_distance, stop as soon as possible - const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); + const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, *stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); const double eps_vel = 0.01; @@ -1138,43 +1157,46 @@ PathWithLaneId GoalPlannerModule::generateStopPath() } // slow down for turn signal, insert stop point to stop_pose - decelerateForTurnSignal(stop_pose, reference_path); - stop_pose_ = stop_pose; + decelerateForTurnSignal(*stop_pose, reference_path); + stop_pose_ = *stop_pose; // for debug wall marker // slow down before the search area. - if (search_start_offset_pose) { - decelerateBeforeSearchStart(*search_start_offset_pose, reference_path); - } else { - // if already passed the search start offset pose, set pull_over_velocity to reference_path. - const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - pull_over_velocity); - for (auto & p : reference_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); - if (min_decel_distance && distance_from_ego < *min_decel_distance) { - continue; - } - p.point.longitudinal_velocity_mps = - std::min(p.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); - } + if (decel_pose) { + decelerateBeforeSearchStart(*decel_pose, reference_path); + return reference_path; } + // if already passed the decel pose, set pull_over_velocity to reference_path. + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + pull_over_velocity); + for (auto & p : reference_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(reference_path, p.point.pose); + if (min_decel_distance && distance_from_ego < *min_decel_distance) { + continue; + } + p.point.longitudinal_velocity_mps = + std::min(p.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); + } return reference_path; } -PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() +PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() const { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; // generate stop reference path - const auto s_current = - lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; + const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); + + const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = - route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); + auto stop_path = route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( @@ -1193,40 +1215,6 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } -void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() -{ - if (!isActivated() || !last_approval_data_) { - return; - } - - // if using arc_path and finishing current_path, get next path - // enough time for turn signal - const bool has_passed_enough_time_from_approval = - (clock_->now() - last_approval_data_->time).seconds() > - planner_data_->parameters.turn_signal_search_time; - if (!has_passed_enough_time_from_approval) { - return; - } - - // require increment only when the time passed is enough - // to prevent increment before driving - // when the end of the current path is close to the current pose - // this value should be `keep_stop_time` in keepStoppedWithCurrentPath - constexpr double keep_current_idx_time = 4.0; - const bool has_passed_enough_time_from_increment = - (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > - keep_current_idx_time; - if (!has_passed_enough_time_from_increment) { - return; - } - - if (!hasFinishedCurrentPath()) { - return; - } - - thread_safe_data_.incrementPathIndex(); -} - bool GoalPlannerModule::isStopped( std::deque & odometry_buffer, const double time) { @@ -1280,18 +1268,60 @@ bool GoalPlannerModule::isStuck() return false; } - return checkCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath()); + if ( + parameters_->use_object_recognition && + checkObjectsCollision( + thread_safe_data_.get_pull_over_path()->getCurrentPath(), + parameters_->object_recognition_collision_check_margin)) { + return true; + } + + if ( + parameters_->use_occupancy_grid_for_path_collision_check && + checkOccupancyGridCollision(thread_safe_data_.get_pull_over_path()->getCurrentPath())) { + return true; + } + + return false; } bool GoalPlannerModule::hasFinishedCurrentPath() { + if (!last_approval_data_) { + return false; + } + + if (!isStopped()) { + return false; + } + + // check if enough time has passed since last approval + // this is necessary to give turn signal for enough time + const bool has_passed_enough_time_from_approval = + (clock_->now() - last_approval_data_->time).seconds() > + planner_data_->parameters.turn_signal_search_time; + if (!has_passed_enough_time_from_approval) { + return false; + } + + // require increment only when the time passed is enough + // to prevent increment before driving + // when the end of the current path is close to the current pose + // this value should be `keep_stop_time` in keepStoppedWithCurrentPath + constexpr double keep_current_idx_time = 4.0; + const bool has_passed_enough_time_from_increment = + (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > + keep_current_idx_time; + if (!has_passed_enough_time_from_increment) { + return false; + } + + // check if self pose is near the end of current path const auto current_path_end = thread_safe_data_.get_pull_over_path()->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; - - return is_near_target && isStopped(); + return tier4_autoware_utils::calcDistance2d(current_path_end, self_pose) < + parameters_->th_arrived_distance; } bool GoalPlannerModule::isOnModifiedGoal() const @@ -1334,9 +1364,8 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over - turn_signal.desired_start_point = last_approval_data_ && status_.get_has_decided_path() - ? last_approval_data_->pose - : current_pose; + turn_signal.desired_start_point = + last_approval_data_ && hasDecidedPath() ? last_approval_data_->pose : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose; @@ -1345,18 +1374,19 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const return turn_signal; } -bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const +bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid_for_path_collision_check && occupancy_grid_map_) { - const bool check_out_of_range = false; - if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { - return true; - } - } - if (!parameters_->use_object_recognition) { + if (!occupancy_grid_map_) { return false; } + const bool check_out_of_range = false; + return occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range); +} +bool GoalPlannerModule::checkObjectsCollision( + const PathWithLaneId & path, const double collision_check_margin, + const bool update_debug_data) const +{ const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); @@ -1371,7 +1401,30 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); } - std::vector ego_polygons_expanded; + /* Expand ego collision check polygon + * - `collision_check_margin` in all directions + * - `extra_stopping_margin` in the moving direction + * - `extra_lateral_margin` in external direction of path curve + * + * + * ^ moving direction + * x + * x + * x + * +----------------------+------x--+ + * | | x | + * | +---------------+ | xx | + * | | | | xx | + * | | ego footprint |xxxxx | + * | | | | | + * | +---------------+ | extra_stopping_margin + * | margin | | + * +----------------------+ | + * | extra_lateral_margin | + * +--------------------------------+ + * + */ + std::vector ego_polygons_expanded{}; const auto curvatures = motion_utils::calcCurvature(path.points); for (size_t i = 0; i < path.points.size(); ++i) { const auto p = path.points.at(i); @@ -1389,16 +1442,16 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( lateral_offset_pose, - planner_data_->parameters.base_link2front + - parameters_->object_recognition_collision_check_margin + extra_stopping_margin, - planner_data_->parameters.base_link2rear + - parameters_->object_recognition_collision_check_margin, - planner_data_->parameters.vehicle_width + - parameters_->object_recognition_collision_check_margin * 2.0 + + planner_data_->parameters.base_link2front + collision_check_margin + extra_stopping_margin, + planner_data_->parameters.base_link2rear + collision_check_margin, + planner_data_->parameters.vehicle_width + collision_check_margin * 2.0 + std::abs(extra_lateral_margin)); ego_polygons_expanded.push_back(ego_polygon); } - debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + if (update_debug_data) { + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + } return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); } @@ -1440,7 +1493,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c return true; } -void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) +void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) const { constexpr double keep_stop_time = 2.0; if (!thread_safe_data_.get_last_path_idx_increment_time()) { @@ -1471,6 +1524,34 @@ double GoalPlannerModule::calcSignedArcLengthFromEgo( path.points, current_pose.position, ego_idx, pose.position, target_idx); } +void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const +{ + // decelerate before the search area start + const auto closest_goal_candidate = + goal_searcher_->getClosetGoalCandidateAlongLanes(thread_safe_data_.get_goal_candidates()); + const auto decel_pose = calcLongitudinalOffsetPose( + pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, + -approximate_pull_over_distance_); + auto & first_path = pull_over_path.partial_paths.front(); + if (decel_pose) { + decelerateBeforeSearchStart(*decel_pose, first_path); + return; + } + + // if already passed the search start offset pose, set pull_over_velocity to first_path. + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); + for (auto & p : first_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); + if (min_decel_distance && distance_from_ego < *min_decel_distance) { + continue; + } + p.point.longitudinal_velocity_mps = std::min( + p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); + } +} + void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const { const double time = planner_data_->parameters.turn_signal_search_time; @@ -1619,7 +1700,7 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { - const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.get_lanes()); + const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); const Pose & start_pose = pull_over_path.start_pose; const Pose & end_pose = pull_over_path.end_pose; @@ -1644,10 +1725,10 @@ bool GoalPlannerModule::isSafePath() const 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 lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, + /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); @@ -1655,11 +1736,10 @@ bool GoalPlannerModule::isSafePath() const const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( thread_safe_data_.get_pull_over_path()->pairs_terminal_velocity_and_accel, - status_.get_current_path_idx()); + thread_safe_data_.get_pull_over_path()->path_idx); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.get_current_path_idx()); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly @@ -1671,16 +1751,16 @@ bool GoalPlannerModule::isSafePath() const ego_seg_idx, is_object_front, limit_to_max_velocity); // filtering objects with velocity, position and class - const auto & filtered_objects = utils::path_safety_checker::filterObjects( + const auto filtered_objects = utils::path_safety_checker::filterObjects( dynamic_object, route_handler, pull_over_lanes, current_pose.position, objects_filtering_params_); // filtering objects based on the current position's lane - const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + 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_.get_prev_is_safe_dynamic_objects() ? 1.0 : parameters_->hysteresis_factor_expand_rate; + prev_data_.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1742,10 +1822,9 @@ void GoalPlannerModule::setDebugData() }; if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = status_.get_has_decided_path() - ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const double z = status_.get_refined_goal_pose().position.z; + const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const double z = planner_data_->route_handler->getGoalPose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); @@ -1777,16 +1856,16 @@ void GoalPlannerModule::setDebugData() "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); - + const double ego_z = planner_data_->self_odometry->pose.pose.position.z; for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { const auto & current_point = ego_polygon.outer().at(ep_idx); const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); marker.points.push_back( - tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), ego_z)); marker.points.push_back( - tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), ego_z)); } } debug_marker_.markers.push_back(marker); @@ -1834,7 +1913,7 @@ void GoalPlannerModule::setDebugData() marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); if (thread_safe_data_.foundPullOverPath()) { marker.text += - " " + std::to_string(status_.get_current_path_idx()) + "/" + + " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx) + "/" + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); } @@ -1877,7 +1956,7 @@ void GoalPlannerModule::printParkingPositionError() const bool GoalPlannerModule::checkOriginalGoalIsInShoulder() const { const auto & route_handler = planner_data_->route_handler; - const Pose & goal_pose = route_handler->getOriginalGoalPose(); + const Pose goal_pose = route_handler->getOriginalGoalPose(); const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index ee7b45de09e93..e772c21331ae8 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -84,6 +84,8 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = getOrDeclareParameter(*node, parameter("regulation.intersection")); + p.regulate_on_traffic_light = + getOrDeclareParameter(*node, parameter("regulation.traffic_light")); // ego vehicle stuck detection p.stop_velocity_threshold = diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 5bffef1a4b421..702ed16166e2b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -19,6 +19,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner/utils/traffic_light_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -38,6 +39,7 @@ namespace behavior_path_planner { using motion_utils::calcSignedArcLength; +using utils::traffic_light::getDistanceToNextTrafficLight; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -1086,6 +1088,19 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( return true; } +bool NormalLaneChange::hasEnoughLengthToTrafficLight( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto dist_to_next_traffic_light = + getDistanceToNextTrafficLight(current_pose, current_lanes); + const auto dist_to_next_traffic_light_from_lc_start_pose = + dist_to_next_traffic_light - path.info.length.prepare; + + return dist_to_next_traffic_light_from_lc_start_pose <= 0.0 || + dist_to_next_traffic_light_from_lc_start_pose >= path.info.length.lane_changing; +} + bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, @@ -1345,6 +1360,12 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "Stop time is over threshold. Allow lane change in intersection."); } + if ( + lane_change_parameters_->regulate_on_traffic_light && + !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { + continue; + } + candidate_paths->push_back(*candidate_path); std::vector filtered_objects = @@ -1615,7 +1636,9 @@ bool NormalLaneChange::calcAbortPath() const double s_start = arc_position.length; double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); - if (route_handler->isInGoalRouteSection(selected_path.info.target_lanes.back())) { + if ( + !reference_lanelets.empty() && + route_handler->isInGoalRouteSection(reference_lanelets.back())) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); const double forward_length = std::max(goal_arc_coordinates.length, s_start); diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 2232c6d750d55..46332e738f82f 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -40,8 +40,6 @@ SideShiftModule::SideShiftModule( const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} { - // If lateral offset is subscribed, it approves side shift module automatically - clearWaitingApproval(); } void SideShiftModule::initVariables() @@ -80,7 +78,7 @@ void SideShiftModule::setParameters(const std::shared_ptr & bool SideShiftModule::isExecutionRequested() const { - if (current_state_ == ModuleStatus::RUNNING) { + if (getCurrentStatus() == ModuleStatus::RUNNING) { return true; } @@ -112,7 +110,7 @@ bool SideShiftModule::isReadyForNextRequest( return false; } -ModuleStatus SideShiftModule::updateState() +bool SideShiftModule::canTransitSuccessState() { // Never return the FAILURE. When the desired offset is zero and the vehicle is in the original // drivable area,this module can stop the computation and return SUCCESS. @@ -150,7 +148,7 @@ ModuleStatus SideShiftModule::updateState() no_shifted_plan); if (no_request && no_shifted_plan && no_offset_diff) { - return ModuleStatus::SUCCESS; + return true; } const auto & current_lanes = utils::getCurrentLanes(planner_data_); @@ -169,7 +167,7 @@ ModuleStatus SideShiftModule::updateState() } else { shift_status_ = SideShiftStatus::AFTER_SHIFT; } - return ModuleStatus::RUNNING; + return false; } void SideShiftModule::updateData() @@ -184,7 +182,7 @@ void SideShiftModule::updateData() } } - if (current_state_ != ModuleStatus::RUNNING && current_state_ != ModuleStatus::IDLE) { + if (getCurrentStatus() != ModuleStatus::RUNNING && getCurrentStatus() != ModuleStatus::IDLE) { return; } @@ -331,8 +329,6 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() prev_output_ = shifted_path; - waitApproval(); - return output; } 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 46db10417d19e..fdc28139aa8f0 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 @@ -33,6 +33,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( std::string ns = "start_planner."; + p.verbose = node->declare_parameter(ns + "verbose"); p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); 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 75562f59a402f..ed70e68414543 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 @@ -128,11 +128,7 @@ void StartPlannerModule::updateData() status_.backward_driving_complete = false; } - const bool has_received_new_route = - !planner_data_->prev_route_id || - *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); - - if (has_received_new_route) { + if (receivedNewRoute()) { status_ = PullOutStatus(); } // check safety status when driving forward @@ -143,6 +139,12 @@ void StartPlannerModule::updateData() } } +bool StartPlannerModule::receivedNewRoute() const +{ + return !planner_data_->prev_route_id || + *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); +} + bool StartPlannerModule::isExecutionRequested() const { if (isModuleRunning()) { @@ -161,7 +163,7 @@ bool StartPlannerModule::isExecutionRequested() const } // Check if the goal is behind the ego vehicle within the same route segment. - if (IsGoalBehindOfEgoInSameRouteSegment()) { + if (isGoalBehindOfEgoInSameRouteSegment()) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Start plan for a backward goal is not supported now"); return false; @@ -228,33 +230,14 @@ bool StartPlannerModule::isExecutionReady() const return true; } -void StartPlannerModule::updateCurrentState() +bool StartPlannerModule::canTransitSuccessState() { - RCLCPP_DEBUG(getLogger(), "START_PLANNER updateCurrentState"); - - const auto print = [this](const auto & from, const auto & to) { - RCLCPP_DEBUG(getLogger(), "[start_planner] Transit from %s to %s.", from.data(), to.data()); - }; - - const auto & from = current_state_; - // current_state_ = updateState(); - - if (isActivated() && !isWaitingApproval()) { - current_state_ = ModuleStatus::RUNNING; - } else { - current_state_ = ModuleStatus::IDLE; - } - - // TODO(someone): move to canTransitSuccessState - if (hasFinishedPullOut()) { - current_state_ = ModuleStatus::SUCCESS; - } - // TODO(someone): move to canTransitSuccessState - if (status_.backward_driving_complete) { - current_state_ = ModuleStatus::SUCCESS; // for breaking loop - } + return hasFinishedPullOut(); +} - print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); +bool StartPlannerModule::canTransitIdleToRunningState() +{ + return isActivated() && !isWaitingApproval(); } BehaviorModuleOutput StartPlannerModule::plan() @@ -278,7 +261,7 @@ BehaviorModuleOutput StartPlannerModule::plan() PathWithLaneId path; // Check if backward motion is finished - if (status_.driving_forward) { + if (status_.driving_forward || status_.backward_driving_complete) { // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); @@ -739,8 +722,6 @@ void StartPlannerModule::updatePullOutStatus() if (isBackwardDrivingComplete()) { updateStatusAfterBackwardDriving(); - // should be moved to transition state - current_state_ = ModuleStatus::SUCCESS; // for breaking loop } else { status_.backward_path = start_planner_utils::getBackwardPath( *route_handler, pull_out_lanes, current_pose, status_.pull_out_start_pose, @@ -763,7 +744,7 @@ void StartPlannerModule::updateStatusAfterBackwardDriving() PathWithLaneId StartPlannerModule::calcBackwardPathFromStartPose() const { - const Pose & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -791,7 +772,7 @@ std::vector StartPlannerModule::searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const { std::vector pull_out_start_pose_candidates{}; - const auto & start_pose = planner_data_->route_handler->getOriginalStartPose(); + const auto start_pose = planner_data_->route_handler->getOriginalStartPose(); const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); @@ -1095,11 +1076,11 @@ bool StartPlannerModule::isSafePath() const is_object_front, limit_to_max_velocity); // filtering objects with velocity, position and class - const auto & filtered_objects = utils::path_safety_checker::filterObjects( + const auto filtered_objects = utils::path_safety_checker::filterObjects( dynamic_object, route_handler, current_lanes, current_pose.position, objects_filtering_params_); // filtering objects based on the current position's lane - const auto & target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( + const auto target_objects_on_lane = utils::path_safety_checker::createTargetObjectsOnLane( current_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = @@ -1139,7 +1120,7 @@ bool StartPlannerModule::isSafePath() const return is_safe_dynamic_objects; } -bool StartPlannerModule::IsGoalBehindOfEgoInSameRouteSegment() const +bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const { const auto & rh = planner_data_->route_handler; @@ -1327,5 +1308,66 @@ void StartPlannerModule::setDebugData() const planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } + if (parameters_->verbose) { + logPullOutStatus(); + } +} + +void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const +{ + const auto logger = getLogger(); + auto logFunc = [&logger, log_level](const char * format, ...) { + char buffer[1024]; + va_list args; + va_start(args, format); + vsnprintf(buffer, sizeof(buffer), format, args); + va_end(args); + switch (log_level) { + case rclcpp::Logger::Level::Debug: + RCLCPP_DEBUG(logger, "%s", buffer); + break; + case rclcpp::Logger::Level::Info: + RCLCPP_INFO(logger, "%s", buffer); + break; + case rclcpp::Logger::Level::Warn: + RCLCPP_WARN(logger, "%s", buffer); + break; + case rclcpp::Logger::Level::Error: + RCLCPP_ERROR(logger, "%s", buffer); + break; + case rclcpp::Logger::Level::Fatal: + RCLCPP_FATAL(logger, "%s", buffer); + break; + default: + RCLCPP_INFO(logger, "%s", buffer); + break; + } + }; + + logFunc("======== PullOutStatus Report ========"); + + logFunc("[Path Info]"); + logFunc(" Current Path Index: %zu", status_.current_path_idx); + + logFunc("[Planner Info]"); + logFunc(" Planner Type: %s", magic_enum::enum_name(status_.planner_type).data()); + + logFunc("[Safety and Direction Info]"); + logFunc(" Found Pull Out Path: %s", status_.found_pull_out_path ? "true" : "false"); + logFunc( + " Is Safe Against Dynamic Objects: %s", status_.is_safe_dynamic_objects ? "true" : "false"); + logFunc( + " Previous Is Safe Dynamic Objects: %s", + status_.prev_is_safe_dynamic_objects ? "true" : "false"); + logFunc(" Driving Forward: %s", status_.driving_forward ? "true" : "false"); + logFunc(" Backward Driving Complete: %s", status_.backward_driving_complete ? "true" : "false"); + logFunc(" Has Stop Point: %s", status_.has_stop_point ? "true" : "false"); + + logFunc("[Module State]"); + logFunc(" isActivated: %s", isActivated() ? "true" : "false"); + logFunc(" isWaitingForApproval: %s", isWaitingApproval() ? "true" : "false"); + logFunc(" ModuleStatus: %s", magic_enum::enum_name(getCurrentStatus())); + + logFunc("======================================="); } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 3008f98331c92..ebc1f686865a6 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -53,7 +53,17 @@ void reuse_previous_poses( prev_poses, 0, ego_point) < 0.0; const auto ego_is_far = !prev_poses.empty() && tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; - if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1) { + // make sure the reused points are not behind the current original drivable area + LineString2d left_bound; + LineString2d right_bound; + for (const auto & p : path.left_bound) left_bound.push_back(convert_point(p)); + for (const auto & p : path.right_bound) right_bound.push_back(convert_point(p)); + LineString2d prev_poses_ls; + for (const auto & p : prev_poses) prev_poses_ls.push_back(convert_point(p.position)); + auto prev_poses_across_bounds = boost::geometry::intersects(left_bound, prev_poses_ls) || + boost::geometry::intersects(right_bound, prev_poses_ls); + + if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1 && !prev_poses_across_bounds) { const auto first_idx = motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); const auto deviation = diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index 7b4d4566bba27..ad841822aeb0d 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -89,7 +89,7 @@ lanelet::ConstLanelets getPullOutLanes( { const double & vehicle_width = planner_data->parameters.vehicle_width; const auto & route_handler = planner_data->route_handler; - const auto & start_pose = planner_data->route_handler->getOriginalStartPose(); + const auto start_pose = planner_data->route_handler->getOriginalStartPose(); lanelet::ConstLanelet current_shoulder_lane; lanelet::ConstLanelets shoulder_lanes; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 19c408359e5f2..0d1eaef0237b3 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1933,22 +1933,33 @@ void makeBoundLongitudinallyMonotonic( return ret; }; - const auto is_monotonic = - [&](const auto & p1, const auto & p2, const auto & p3, const auto is_points_left) { - const auto p1_to_p2 = calcAzimuthAngle(p1, p2); - const auto p2_to_p3 = calcAzimuthAngle(p2, p3); + const auto is_non_monotonic = [&]( + const auto & base_pose, const auto & point, + const auto is_points_left, const auto is_reverse) { + const auto p_transformed = tier4_autoware_utils::inverseTransformPoint(point, base_pose); + if (p_transformed.x < 0.0 && p_transformed.y > 0.0) { + return is_points_left && !is_reverse; + } - const auto theta = normalizeRadian(p1_to_p2 - p2_to_p3); + if (p_transformed.x < 0.0 && p_transformed.y < 0.0) { + return !is_points_left && !is_reverse; + } - return (is_points_left && 0 < theta) || (!is_points_left && theta < 0); - }; + if (p_transformed.x > 0.0 && p_transformed.y > 0.0) { + return is_points_left && is_reverse; + } + + if (p_transformed.x > 0.0 && p_transformed.y < 0.0) { + return !is_points_left && is_reverse; + } + + return false; + }; // define a function to remove non monotonic point on bound const auto remove_non_monotonic_point = [&](const auto & bound_with_pose, const bool is_reverse) { std::vector monotonic_bound; - const bool is_points_left = is_reverse ? !is_bound_left : is_bound_left; - size_t bound_idx = 0; while (true) { monotonic_bound.push_back(bound_with_pose.at(bound_idx)); @@ -1961,13 +1972,12 @@ void makeBoundLongitudinallyMonotonic( // opposite. const double lat_offset = is_bound_left ? 100.0 : -100.0; - const auto p_bound_1 = getPoint(bound_with_pose.at(bound_idx)); - const auto p_bound_2 = getPoint(bound_with_pose.at(bound_idx + 1)); + const auto p_bound_1 = getPose(bound_with_pose.at(bound_idx)); + const auto p_bound_2 = getPose(bound_with_pose.at(bound_idx + 1)); - const auto p_bound_offset = - calcOffsetPose(getPose(bound_with_pose.at(bound_idx)), 0.0, lat_offset, 0.0); + const auto p_bound_offset = calcOffsetPose(p_bound_1, 0.0, lat_offset, 0.0); - if (!is_monotonic(p_bound_1, p_bound_2, p_bound_offset.position, is_points_left)) { + if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_bound_left, is_reverse)) { bound_idx++; continue; } @@ -1975,7 +1985,7 @@ void makeBoundLongitudinallyMonotonic( // skip non monotonic points for (size_t i = bound_idx + 1; i < bound_with_pose.size() - 1; ++i) { const auto intersect_point = intersect( - p_bound_1, p_bound_offset.position, bound_with_pose.at(i).position, + p_bound_1.position, p_bound_offset.position, bound_with_pose.at(i).position, bound_with_pose.at(i + 1).position); if (intersect_point) { diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index c74c5b3d87c1d..092f1d32b27b3 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -17,6 +17,7 @@ use_predicted_paths: true # 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. predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. + distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane overlap: minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered @@ -26,6 +27,7 @@ skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed precision: 0.1 # [m] precision when inserting a stop pose in the path distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane + min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap velocity: 2.0 # [m/s] slowdown velocity diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 5d22f01222919..0c9e6448bb374 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -29,6 +29,11 @@ namespace behavior_velocity_planner::out_of_lane { +/// @brief estimate whether ego can decelerate without breaking the deceleration limit +/// @details assume ego wants to reach the target point at the target velocity +/// @param [in] ego_data ego data +/// @param [in] point target point +/// @param [in] target_vel target_velocity bool can_decelerate( const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) { @@ -40,9 +45,16 @@ bool can_decelerate( return acc_to_target_vel < std::abs(ego_data.max_decel); } +/// @brief calculate the last pose along the path where ego does not overlap the lane to avoid +/// @param [in] ego_data ego data +/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) +/// @param [in] footprint the ego footprint +/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits +/// @param [in] params parameters +/// @return the last pose that is not out of lane (if found) std::optional calculate_last_in_lane_pose( const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, - const PlannerParam & params) + const std::optional & prev_slowdown_point, const PlannerParam & params) { const auto from_arc_length = motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); @@ -53,12 +65,17 @@ std::optional calculate_last_in_lane_pose( // TODO(Maxime): binary search interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); const auto respect_decel_limit = - !params.skip_if_over_max_decel || + !params.skip_if_over_max_decel || prev_slowdown_point || can_decelerate(ego_data, interpolated_point, decision.velocity); - if ( - respect_decel_limit && !boost::geometry::overlaps( - project_to_pose(footprint, interpolated_point.point.pose), - decision.lane_to_avoid.polygon2d().basicPolygon())) + const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.point.pose); + const auto is_overlap_lane = boost::geometry::overlaps( + interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); + const auto is_overlap_extra_lane = + prev_slowdown_point && + boost::geometry::overlaps( + interpolated_footprint, + prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); + if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) return interpolated_point; } return std::nullopt; @@ -67,10 +84,12 @@ std::optional calculate_last_in_lane_pose( /// @brief calculate the slowdown point to insert in the path /// @param ego_data ego data (path, velocity, etc) /// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param prev_slowdown_point previously calculated slowdown point /// @param params parameters /// @return optional slowdown point to insert in the path std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params) + const EgoData & ego_data, const std::vector & decisions, + const std::optional prev_slowdown_point, PlannerParam params) { params.extra_front_offset += params.dist_buffer; const auto base_footprint = make_base_footprint(params); @@ -78,7 +97,7 @@ std::optional calculate_slowdown_point( // search for the first slowdown decision for which a stop point can be inserted for (const auto & decision : decisions) { const auto last_in_lane_pose = - calculate_last_in_lane_pose(ego_data, decision, base_footprint, params); + calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; } return std::nullopt; diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 0574a5226dc4a..489bb3f5abc78 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -34,13 +34,12 @@ visualization_msgs::msg::Marker get_base_marker() base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); - base_marker.lifetime = rclcpp::Duration::from_seconds(0.5); return base_marker; } } // namespace void add_footprint_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z) + const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = "footprints"; @@ -54,12 +53,14 @@ void add_footprint_markers( debug_marker.id++; debug_marker.points.clear(); } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); } void add_current_overlap_marker( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z) + const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = "current_overlap"; @@ -82,12 +83,14 @@ void add_current_overlap_marker( debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); } void add_lanelet_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color) + const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = ns; @@ -103,6 +106,82 @@ void add_lanelet_markers( debug_marker_array.markers.push_back(debug_marker); debug_marker.id++; } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); +} + +void add_range_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, + const double z, const size_t prev_nb) +{ + auto debug_marker = get_base_marker(); + debug_marker.ns = "ranges"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); + for (const auto & range : ranges) { + debug_marker.points.clear(); + debug_marker.points.push_back( + path.points[first_ego_idx + range.entering_path_idx].point.pose.position); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + for (const auto & overlap : range.debug.overlaps) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); + } + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.exiting_point.x(), range.exiting_point.y(), z)); + debug_marker.points.push_back( + path.points[first_ego_idx + range.exiting_path_idx].point.pose.position); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + debug_marker.action = debug_marker.DELETE; + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); + debug_marker.action = debug_marker.ADD; + debug_marker.id = 0; + debug_marker.ns = "decisions"; + debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); + debug_marker.points.clear(); + for (const auto & range : ranges) { + debug_marker.type = debug_marker.LINE_STRIP; + if (range.debug.decision) { + debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( + range.entering_point.x(), range.entering_point.y(), z)); + debug_marker.points.push_back( + range.debug.object->kinematics.initial_pose_with_covariance.pose.position); + } + debug_marker_array.markers.push_back(debug_marker); + debug_marker.points.clear(); + debug_marker.id++; + + debug_marker.type = debug_marker.TEXT_VIEW_FACING; + debug_marker.pose.position.x = range.entering_point.x(); + debug_marker.pose.position.y = range.entering_point.y(); + debug_marker.pose.position.z = z; + std::stringstream ss; + ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time + << "\n"; + if (range.debug.object) { + debug_marker.pose.position.x += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; + debug_marker.pose.position.y += + range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; + debug_marker.pose.position.x /= 2; + debug_marker.pose.position.y /= 2; + ss << "Obj: " << range.debug.times.object.enter_time << " - " + << range.debug.times.object.exit_time << "\n"; + } + debug_marker.scale.z = 1.0; + debug_marker.text = ss.str(); + debug_marker_array.markers.push_back(debug_marker); + debug_marker.id++; + } + for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) + debug_marker_array.markers.push_back(debug_marker); } } // namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 06bc6d935f310..2b6b65638ec40 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -29,27 +29,41 @@ namespace behavior_velocity_planner::out_of_lane::debug /// @param [inout] debug_marker_array marker array /// @param [in] footprints footprints to turn into markers /// @param [in] z z value to use for the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_footprint_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z); + const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb); /// @brief add footprint markers to the given marker array /// @param [inout] debug_marker_array marker array /// @param [in] current_footprint footprint to turn into a marker /// @param [in] current_overlapped_lanelets lanelets to turn into markers /// @param [in] z z value to use for the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_current_overlap_marker( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z); + const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb); /// @brief add footprint markers to the given marker array /// @param [inout] debug_marker_array marker array /// @param [in] lanelets lanelets to turn into markers /// @param [in] ns namespace of the markers /// @param [in] color namespace of the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_lanelet_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color); + const std_msgs::msg::ColorRGBA & color, const size_t prev_nb); +/// @brief add ranges markers to the given marker array +/// @param [inout] debug_marker_array marker array +/// @param [in] ranges ranges to turn into markers +/// @param [in] path modified ego path that was used to calculate the ranges +/// @param [in] first_path_idx first idx of ego on the path +/// @param [in] z z value to use for the markers +/// @param [in] prev_nb previous number of markers (used to delete the extra ones) +void add_range_markers( + visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, + const double z, const size_t prev_nb); } // namespace behavior_velocity_planner::out_of_lane::debug #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 78d9b73c86a17..be72109db4548 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -26,7 +26,6 @@ #include #include -#include #include #include #include @@ -63,7 +62,8 @@ bool object_is_incoming( std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const rclcpp::Logger & logger) + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger) { // skip the dynamic object if it is not in a lane preceding the overlapped lane // lane changes are intentionally not considered @@ -72,8 +72,7 @@ std::optional> object_time_to_range( object.kinematics.initial_pose_with_covariance.pose.position.y); if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - const auto max_deviation = object.shape.dimensions.y + range.inside_distance; - + const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; auto worst_enter_time = std::optional(); auto worst_exit_time = std::optional(); @@ -308,7 +307,8 @@ bool should_not_enter( // skip objects that are already on the interval const auto enter_exit_time = params.objects_use_predicted_paths - ? object_time_to_range(object, range, inputs.route_handler, logger) + ? object_time_to_range( + object, range, inputs.route_handler, params.objects_dist_buffer, logger) : object_time_to_range(object, range, inputs, logger); if (!enter_exit_time) { RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); @@ -317,27 +317,14 @@ bool should_not_enter( range_times.object.enter_time = enter_exit_time->first; range_times.object.exit_time = enter_exit_time->second; - if (will_collide_on_range(range_times, params, logger)) return true; - } - return false; -} - -OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs) -{ - OverlapRange preceding_range = range; - bool found_preceding_range = true; - while (found_preceding_range) { - found_preceding_range = false; - for (const auto & other_range : inputs.ranges) { - if ( - other_range.entering_path_idx < preceding_range.entering_path_idx && - other_range.exiting_path_idx >= preceding_range.entering_path_idx) { - preceding_range = other_range; - found_preceding_range = true; - } + if (will_collide_on_range(range_times, params, logger)) { + range.debug.times = range_times; + range.debug.object = object; + return true; } } - return preceding_range; + range.debug.times = range_times; + return false; } void set_decision_velocity( @@ -375,6 +362,7 @@ std::vector calculate_decisions( for (const auto & range : inputs.ranges) { if (range.entering_path_idx == 0UL) continue; // skip if we already entered the range const auto optional_decision = calculate_decision(range, inputs, params, logger); + range.debug.decision = optional_decision; if (optional_decision) decisions.push_back(*optional_decision); } return decisions; diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 2898595c2e74c..0612cc041c1ef 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -31,16 +31,6 @@ namespace behavior_velocity_planner::out_of_lane { -struct EnterExitTimes -{ - double enter_time; - double exit_time; -}; -struct RangeTimes -{ - EnterExitTimes ego; - EnterExitTimes object; -}; /// @brief calculate the distance along the ego path between ego and some target path index /// @param [in] ego_data data related to the ego vehicle /// @param [in] target_idx target ego path index @@ -64,13 +54,15 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const rclcpp::Logger & logger); + const std::shared_ptr route_handler, const double dist_buffer, + const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range /// @param [in] object dynamic object /// @param [in] range overlapping range /// @param [in] inputs information used to take decisions (ranges, ego and objects data, route /// handler, lanelets) +/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range /// @param [in] logger ros logger /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. @@ -94,14 +86,6 @@ bool will_collide_on_range( bool should_not_enter( const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief find the most preceding range -/// @details the most preceding range is the first range in a succession of ranges with overlapping -/// enter/exit indexes. -/// @param [in] range range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @return most preceding range -OverlapRange find_most_preceding_range(const OverlapRange & range, const DecisionInputs & inputs); /// @brief set the velocity of a decision (or unset it) based on the distance away from the range /// @param [out] decision decision to update (either set its velocity or unset the decision) /// @param [in] distance distance between ego and the range corresponding to the decision diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 2422c6fe84e56..23de809e429ec 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -50,6 +50,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".objects.use_predicted_paths"); pp.objects_min_confidence = getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); + pp.objects_dist_buffer = getOrDeclareParameter(node, ns + ".objects.distance_buffer"); pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); @@ -57,6 +58,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.skip_if_over_max_decel = getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); + pp.min_decision_duration = getOrDeclareParameter(node, ns + ".action.min_duration"); pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp index 98a9e4404e013..6f74f42af2056 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -23,7 +23,6 @@ #include #include -#include namespace behavior_velocity_planner::out_of_lane { @@ -76,10 +75,12 @@ OverlapRanges calculate_overlapping_ranges( { OverlapRanges ranges; OtherLane other_lane(lanelet); + std::vector overlaps; for (auto i = 0UL; i < path_footprints.size(); ++i) { const auto overlap = calculate_overlap(path_footprints[i], path_lanelets, lanelet); const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; if (has_overlap) { // open/update the range + overlaps.push_back(overlap); if (!other_lane.range_is_open) { other_lane.first_range_bound.index = i; other_lane.first_range_bound.point = overlap.min_overlap_point; @@ -94,10 +95,16 @@ OverlapRanges calculate_overlapping_ranges( other_lane.last_range_bound.inside_distance = overlap.inside_distance; } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); } } // close the range if it is still open - if (other_lane.range_is_open) ranges.push_back(other_lane.close_range()); + if (other_lane.range_is_open) { + ranges.push_back(other_lane.close_range()); + ranges.back().debug.overlaps = overlaps; + overlaps.clear(); + } return ranges; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp index ab4d31480e1cb..2b0830acc28cc 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -21,21 +21,11 @@ #include -#include #include namespace behavior_velocity_planner::out_of_lane { -/// @brief representation of an overlap between the ego footprint and some other lane -struct Overlap -{ - double inside_distance = 0.0; ///!< distance inside the overlap - double min_arc_length = std::numeric_limits::infinity(); - double max_arc_length = 0.0; - lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length - lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length -}; /// @brief calculate the overlap between the given footprint and lanelet /// @param [in] path_footprint footprint used to calculate the overlap /// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path @@ -59,7 +49,8 @@ OverlapRanges calculate_overlapping_ranges( /// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path /// @param [in] lanelets lanelets used to calculate the overlaps /// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelets +/// @return the overlapping ranges found between the footprints and the lanelets, sorted by +/// increasing arc length along the path OverlapRanges calculate_overlapping_ranges( const std::vector & path_footprints, const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 1487d9e63c8b0..d739c6440ae75 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -55,8 +55,7 @@ OutOfLaneModule::OutOfLaneModule( velocity_factor_.init(VelocityFactor::UNKNOWN); } -bool OutOfLaneModule::modifyPathVelocity( - PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) { debug_data_.reset_data(); *stop_reason = planning_utils::initializeStopReason(StopReason::OUT_OF_LANE); @@ -68,6 +67,7 @@ bool OutOfLaneModule::modifyPathVelocity( ego_data.path.points = path->points; ego_data.first_path_idx = motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + motion_utils::removeOverlapPoints(ego_data.path.points); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; stopwatch.tic("calculate_path_footprints"); @@ -87,6 +87,8 @@ bool OutOfLaneModule::modifyPathVelocity( debug_data_.path_lanelets = path_lanelets; debug_data_.ignored_lanelets = ignored_lanelets; debug_data_.other_lanelets = other_lanelets; + debug_data_.path = ego_data.path; + debug_data_.first_path_idx = ego_data.first_path_idx; if (params_.skip_if_already_overlapping) { debug_data_.current_footprint = current_ego_footprint; @@ -113,33 +115,69 @@ bool OutOfLaneModule::modifyPathVelocity( inputs.objects = filter_predicted_objects(*planner_data_->predicted_objects, ego_data, params_); inputs.route_handler = planner_data_->route_handler_; inputs.lanelets = other_lanelets; - auto decisions = calculate_decisions(inputs, params_, logger_); + const auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); - const auto point_to_insert = calculate_slowdown_point(ego_data, decisions, params_); + if ( // reset the previous inserted point if the timer expired + prev_inserted_point_ && + (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) + prev_inserted_point_.reset(); + auto point_to_insert = + calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); stopwatch.tic("insert_slowdown_points"); debug_data_.slowdowns.clear(); + if ( // reset the timer if there is no previous inserted point or if we avoid the same lane + point_to_insert && + (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == + point_to_insert->slowdown.lane_to_avoid.id())) + prev_inserted_point_time_ = clock_->now(); + // reuse previous stop point if there is no new one or if its velocity is not higher than the new + // one and its arc length is lower + const auto should_use_prev_inserted_point = [&]() { + if ( + point_to_insert && prev_inserted_point_ && + prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { + const auto arc_length = motion_utils::calcSignedArcLength( + path->points, 0LU, point_to_insert->point.point.pose.position); + const auto prev_arc_length = motion_utils::calcSignedArcLength( + path->points, 0LU, prev_inserted_point_->point.point.pose.position); + return prev_arc_length < arc_length; + } + return !point_to_insert && prev_inserted_point_; + }(); + if (should_use_prev_inserted_point) { + // if the path changed the prev point is no longer on the path so we project it + const auto insert_arc_length = motion_utils::calcSignedArcLength( + path->points, 0LU, prev_inserted_point_->point.point.pose.position); + prev_inserted_point_->point.point.pose = + motion_utils::calcInterpolatedPose(path->points, insert_arc_length); + point_to_insert = prev_inserted_point_; + } if (point_to_insert) { + prev_inserted_point_ = point_to_insert; + RCLCPP_INFO(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); debug_data_.slowdowns = {*point_to_insert}; auto path_idx = motion_utils::findNearestSegmentIndex( path->points, point_to_insert->point.point.pose.position) + 1; planning_utils::insertVelocity( - *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx, - params_.precision); + *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx); if (point_to_insert->slowdown.velocity == 0.0) { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = point_to_insert->point.point.pose; stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( - ego_data.path.points, ego_data.pose.position, point_to_insert->point.point.pose.position); + path->points, ego_data.pose.position, point_to_insert->point.point.pose.position); planning_utils::appendStopReason(stop_factor, stop_reason); } velocity_factor_.set( path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, VelocityFactor::UNKNOWN); + } else if (!decisions.empty()) { + RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); } const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); + debug_data_.ranges = inputs.ranges; const auto total_time_us = stopwatch.toc(); RCLCPP_DEBUG( @@ -160,18 +198,23 @@ MarkerArray OutOfLaneModule::createDebugMarkerArray() constexpr auto z = 0.0; MarkerArray debug_marker_array; - debug::add_footprint_markers(debug_marker_array, debug_data_.footprints, z); + debug::add_footprint_markers( + debug_marker_array, debug_data_.footprints, z, debug_data_.prev_footprints); debug::add_current_overlap_marker( - debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z); + debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z, + debug_data_.prev_current_overlapped_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data_.path_lanelets, "path_lanelets", - tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5)); + tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), debug_data_.prev_path_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data_.ignored_lanelets, "ignored_lanelets", - tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5)); + tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data_.prev_ignored_lanelets); debug::add_lanelet_markers( debug_marker_array, debug_data_.other_lanelets, "other_lanelets", - tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5)); + tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data_.prev_other_lanelets); + debug::add_range_markers( + debug_marker_array, debug_data_.ranges, debug_data_.path, debug_data_.first_path_idx, z, + debug_data_.prev_ranges); return debug_marker_array; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index cdf8eef8f277b..6133bb1ea0d6e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -48,9 +49,11 @@ class OutOfLaneModule : public SceneModuleInterface motion_utils::VirtualWalls createVirtualWalls() override; private: - // Parameter PlannerParam params_; + std::optional prev_inserted_point_{}; + rclcpp::Time prev_inserted_point_time_{}; + protected: int64_t module_id_{}; diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 64d88f15e6966..34bd52634ce7b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -24,7 +24,9 @@ #include #include +#include #include +#include #include #include #include @@ -47,6 +49,8 @@ struct PlannerParam bool objects_use_predicted_paths; // whether to use the objects' predicted paths double objects_min_vel; // [m/s] objects lower than this velocity will be ignored double objects_min_confidence; // minimum confidence to consider a predicted path + double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in + // the other lane double overlap_extra_length; // [m] extra length to add around an overlap range double overlap_min_dist; // [m] min distance inside another lane to consider an overlap @@ -56,7 +60,8 @@ struct PlannerParam double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; - double precision; // [m] precision when inserting a stop pose in the path + double precision; // [m] precision when inserting a stop pose in the path + double min_decision_duration; // [s] minimum duration needed a decision can be canceled // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -68,17 +73,16 @@ struct PlannerParam double extra_left_offset; // [m] extra left distance }; -/// @brief range along the path where ego overlaps another lane -struct OverlapRange +struct EnterExitTimes { - lanelet::ConstLanelet lane; - size_t entering_path_idx{}; - size_t exiting_path_idx{}; - lanelet::BasicPoint2d entering_point; // pose of the overlapping point closest along the lane - lanelet::BasicPoint2d exiting_point; // pose of the overlapping point furthest along the lane - double inside_distance{}; // [m] how much ego footprint enters the lane + double enter_time{}; + double exit_time{}; +}; +struct RangeTimes +{ + EnterExitTimes ego{}; + EnterExitTimes object{}; }; -using OverlapRanges = std::vector; /// @brief action taken by the "out of lane" module struct Slowdown @@ -102,6 +106,35 @@ struct RangeBound double arc_length; double inside_distance; }; + +/// @brief representation of an overlap between the ego footprint and some other lane +struct Overlap +{ + double inside_distance = 0.0; ///!< distance inside the overlap + double min_arc_length = std::numeric_limits::infinity(); + double max_arc_length = 0.0; + lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length + lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length +}; + +/// @brief range along the path where ego overlaps another lane +struct OverlapRange +{ + lanelet::ConstLanelet lane; + size_t entering_path_idx{}; + size_t exiting_path_idx{}; + lanelet::BasicPoint2d entering_point; // pose of the overlapping point closest along the lane + lanelet::BasicPoint2d exiting_point; // pose of the overlapping point furthest along the lane + double inside_distance{}; // [m] how much ego footprint enters the lane + mutable struct + { + std::vector overlaps; + std::optional decision; + RangeTimes times; + std::optional object{}; + } debug; +}; +using OverlapRanges = std::vector; /// @brief representation of a lane and its current overlap range struct OtherLane { @@ -164,12 +197,32 @@ struct DebugData lanelet::ConstLanelets path_lanelets; lanelet::ConstLanelets ignored_lanelets; lanelet::ConstLanelets other_lanelets; + autoware_auto_planning_msgs::msg::PathWithLaneId path; + size_t first_path_idx; + + size_t prev_footprints = 0; + size_t prev_slowdowns = 0; + size_t prev_ranges = 0; + size_t prev_current_overlapped_lanelets = 0; + size_t prev_ignored_lanelets = 0; + size_t prev_path_lanelets = 0; + size_t prev_other_lanelets = 0; void reset_data() { + prev_footprints = footprints.size(); footprints.clear(); + prev_slowdowns = slowdowns.size(); slowdowns.clear(); + prev_ranges = ranges.size(); ranges.clear(); + prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); current_overlapped_lanelets.clear(); + prev_ignored_lanelets = ignored_lanelets.size(); + ignored_lanelets.clear(); + prev_path_lanelets = path_lanelets.size(); + path_lanelets.clear(); + prev_other_lanelets = other_lanelets.size(); + other_lanelets.clear(); } }; diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index fb3c49bb750a6..7bd27fca3407c 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -9,6 +9,7 @@ Makoto Kurihara Tomoya Kimura Shumpei Wakabayashi + Takayuki Murooka Apache License 2.0 diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 2ba0a4c1d46f3..ccca5e97d2b38 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -46,6 +46,7 @@ VelocityLimit getHardestLimit( // find hardest max velocity if (max_velocity < hardest_max_velocity) { hardest_limit.stamp = limit.second.stamp; + hardest_limit.sender = limit.first; hardest_limit.max_velocity = max_velocity; hardest_max_velocity = max_velocity; } diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 347d19f631588..7be91d67cb945 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -42,6 +42,7 @@ #include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary #include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "visualization_msgs/msg/marker_array.hpp" #include #include @@ -63,6 +64,7 @@ using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32Stamped; // temporary using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary using tier4_planning_msgs::msg::VelocityLimit; // temporary +using visualization_msgs::msg::MarkerArray; struct Motion { @@ -80,6 +82,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_odometry_; rclcpp::Subscription::SharedPtr sub_current_acceleration_; @@ -95,6 +98,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node double max_velocity_with_deceleration_; // maximum velocity with deceleration // for external velocity limit double wheelbase_; // wheelbase + double base_link2front_; // base_link to front TrajectoryPoints prev_output_; // previously published trajectory @@ -153,6 +157,7 @@ class MotionVelocitySmootherNode : public rclcpp::Node { double velocity{0.0}; // current external_velocity_limit double dist{0.0}; // distance to set external velocity limit + std::string sender{""}; }; ExternalVelocityLimit external_velocity_limit_; // velocity and distance constraint of external velocity limit diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 6b5da01b9cb09..a27e02c15710f 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -14,6 +14,7 @@ #include "motion_velocity_smoother/motion_velocity_smoother_node.hpp" +#include "motion_utils/marker/marker_helper.hpp" #include "motion_velocity_smoother/smoother/jerk_filtered_smoother.hpp" #include "motion_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp" #include "motion_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp" @@ -41,6 +42,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions // set common params const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; + base_link2front_ = vehicle_info.max_longitudinal_offset_m; initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); @@ -49,6 +51,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions // publishers, subscribers pub_trajectory_ = create_publisher("~/output/trajectory", 1); + pub_virtual_wall_ = create_publisher("~/virtual_wall", 1); pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); @@ -329,6 +332,9 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit() return; } + // sender + external_velocity_limit_.sender = external_velocity_limit_ptr_->sender; + // on the first time, apply directly if (prev_output_.empty() || !current_closest_point_from_prev_output_) { external_velocity_limit_.velocity = external_velocity_limit_ptr_->max_velocity; @@ -890,6 +896,14 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t trajectory_utils::applyMaximumVelocityLimit( *inserted_index, traj.size(), external_velocity_limit_.velocity, traj); + // create virtual wall + if (std::abs(external_velocity_limit_.velocity) < 1e-3) { + const auto virtual_wall_marker = motion_utils::createStopVirtualWallMarker( + traj.at(*inserted_index).pose, external_velocity_limit_.sender, this->now(), 0, + base_link2front_); + pub_virtual_wall_->publish(virtual_wall_marker); + } + RCLCPP_DEBUG( get_logger(), "externalVelocityLimit : limit_vel = %.3f", external_velocity_limit_.velocity); } diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 0c5441c1ad9c8..1491f8ffea36e 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -98,6 +98,8 @@ Ego vehicle pitch angle is calculated in the following manner. ![pitch calculation](./media/pitch-calculation.drawio.svg) +NOTE: driving against the line direction (as depicted in image's bottom row) is not supported and only shown for illustration purposes. + ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 07a5d34f63c39..f0b126cbc61be 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -210,7 +210,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt // control mode current_control_mode_.mode = ControlModeReport::AUTONOMOUS; - current_manual_gear_cmd_.command = GearCommand::DRIVE; + current_manual_gear_cmd_.command = GearCommand::PARK; } void SimplePlanningSimulator::initialize_vehicle_model() @@ -331,8 +331,9 @@ void SimplePlanningSimulator::on_timer() // calculate longitudinal acceleration by slope constexpr double gravity_acceleration = -9.81; - const double ego_pitch_angle = enable_road_slope_simulation_ ? calculate_ego_pitch() : 0.0; - const double acc_by_slope = gravity_acceleration * std::sin(ego_pitch_angle); + const double ego_pitch_angle = calculate_ego_pitch(); + const double slope_angle = enable_road_slope_simulation_ ? -ego_pitch_angle : 0.0; + const double acc_by_slope = gravity_acceleration * std::sin(slope_angle); // update vehicle dynamics { diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index c00203dbb6073..5672d13dcb43f 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -50,6 +50,19 @@ error_rate: 1.0 timeout: 1.0 +- module: localization + mode: [online, logging_simulation] + type: autonomous + args: + node_name_suffix: pose_estimator_pose + topic: /localization/pose_estimator/pose_with_covariance + topic_type: geometry_msgs/msg/PoseWithCovarianceStamped + best_effort: false + transient_local: false + warn_rate: 5.0 + error_rate: 1.0 + timeout: 1.0 + - module: perception mode: [online, logging_simulation] type: launch