diff --git a/.github/workflows/beta-to-tier4-main-sync.yaml b/.github/workflows/beta-to-tier4-main-sync.yaml new file mode 100644 index 0000000000000..53f40f1e811ab --- /dev/null +++ b/.github/workflows/beta-to-tier4-main-sync.yaml @@ -0,0 +1,34 @@ +name: beta-to-tier4-main-sync + +on: + workflow_dispatch: + inputs: + source_branch: + description: Source branch + required: true + type: string + +jobs: + sync-beta-branch: + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Run sync-branches + uses: autowarefoundation/autoware-github-actions/sync-branches@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + base-branch: tier4/main + sync-pr-branch: beta-to-tier4-main-sync + sync-target-repository: https://github.com/tier4/autoware.universe.git + sync-target-branch: ${{ inputs.source_branch }} + pr-title: "chore: sync beta branch ${{ inputs.source_branch }} with tier4/main" + pr-labels: | + bot + sync-beta-branch + auto-merge-method: merge diff --git a/.github/workflows/create-awf-latest.yml b/.github/workflows/create-awf-latest.yml new file mode 100644 index 0000000000000..191b40ee69b05 --- /dev/null +++ b/.github/workflows/create-awf-latest.yml @@ -0,0 +1,25 @@ +name: create-awf-latest + +on: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + create-awf-latest: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v2 + with: + fetch-depth: 0 + - name: Commit Results + run: | + git config --local user.email "action@github.com" + git config --local user.name "GitHub Action" + + git checkout awf-latest + git remote add awf https://github.com/autowarefoundation/autoware.universe + git fetch awf main + git rebase awf/main + + git push origin awf-latest --force diff --git a/.github/workflows/dispatch-push-event.yaml b/.github/workflows/dispatch-push-event.yaml new file mode 100644 index 0000000000000..492083c8038f6 --- /dev/null +++ b/.github/workflows/dispatch-push-event.yaml @@ -0,0 +1,45 @@ +name: dispatch-push-event +on: + push: + +jobs: + search-dispatch-repo: + runs-on: ubuntu-latest + strategy: + matrix: + include: + - { version: beta/v0.3.**, dispatch-repo: pilot-auto.x1.eve } + outputs: + dispatch-repo: ${{ steps.search-dispatch-repo.outputs.value }} + steps: + - name: Search dispatch repo + id: search-dispatch-repo + run: | + if [[ ${{ github.ref_name }} =~ ${{ matrix.version }} ]]; then + echo ::set-output name=value::"${{ matrix.dispatch-repo }}" + echo "Detected beta branch: ${{ github.ref_name }}" + echo "Dispatch repository: ${{ matrix.dispatch-repo }}" + fi + + dispatch-push-event: + runs-on: ubuntu-latest + needs: search-dispatch-repo + if: ${{ needs.search-dispatch-repo.outputs.dispatch-repo != '' }} + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.INTERNAL_APP_ID }} + private_key: ${{ secrets.INTERNAL_PRIVATE_KEY }} + + # 注意: workflow_dispatchで指定するブランチはmain固定となっているため、dispatch-repoのmainブランチにupdate-beta-branch.yamlが存在することが前提条件。 + - name: Dispatch the update-beta-branch workflow + run: | + curl -L \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: Bearer ${{ steps.generate-token.outputs.token }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + https://api.github.com/repos/tier4/${{ needs.search-dispatch-repo.outputs.dispatch-repo }}/actions/workflows/update-beta-branch.yaml/dispatches \ + -d '{"ref":"main"}' diff --git a/.github/workflows/dispatch-release-note.yaml b/.github/workflows/dispatch-release-note.yaml new file mode 100644 index 0000000000000..0245e2b929cd8 --- /dev/null +++ b/.github/workflows/dispatch-release-note.yaml @@ -0,0 +1,46 @@ +name: dispatch-release-note +on: + push: + branches: + - beta/v* + - tier4/main + tags: + - v* + workflow_dispatch: + inputs: + beta-branch-or-tag-name: + description: The name of the beta branch or tag to write release note + type: string + required: true +jobs: + dispatch-release-note: + runs-on: ubuntu-latest + name: release-repository-dispatch + steps: + - name: Set tag name + id: set-tag-name + run: | + if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then + REF_NAME="${{ github.event.inputs.beta-branch-or-tag-name }}" + else + REF_NAME="${{ github.ref_name }}" + fi + echo ::set-output name=ref-name::"$REF_NAME" + echo ::set-output name=tag-name::"${REF_NAME#beta/}" + + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - name: Repository dispatch for release note + run: | + curl \ + -X POST \ + -H "Accept: application/vnd.github+json" \ + -H "Authorization: token ${{ steps.generate-token.outputs.token }}" \ + -H "X-GitHub-Api-Version: 2022-11-28" \ + "https://api.github.com/repos/tier4/update-release-notes/dispatches" \ + -d '{"event_type":"${{ steps.set-tag-name.outputs.ref-name }}"}' diff --git a/.github/workflows/slack-send.yaml b/.github/workflows/slack-send.yaml new file mode 100644 index 0000000000000..973b08674f7e8 --- /dev/null +++ b/.github/workflows/slack-send.yaml @@ -0,0 +1,25 @@ +name: slack-send +on: + workflow_run: + workflows: + - build-and-test + types: + - completed + +jobs: + on-failure: + if: ${{ github.event.workflow_run.conclusion == 'failure' }} + runs-on: ubuntu-latest + steps: + - name: Check out repository + uses: actions/checkout@v3 + + - name: Send to Slack workflow + uses: slackapi/slack-github-action@v1 + with: + payload: | + { + "workflow-url": "${{ github.event.workflow_run.html_url }}" + } + env: + SLACK_WEBHOOK_URL: ${{ secrets.SLACK_WORKFLOW_WEBHOOK_URL }} diff --git a/.github/workflows/sync-upstream.yaml b/.github/workflows/sync-upstream.yaml new file mode 100644 index 0000000000000..90420ec404042 --- /dev/null +++ b/.github/workflows/sync-upstream.yaml @@ -0,0 +1,49 @@ +name: sync-upstream + +on: + workflow_dispatch: + inputs: + target_branch: + description: Target branch + required: true + type: string + +jobs: + sync-upstream: + runs-on: ubuntu-latest + steps: + - name: Generate token + id: generate-token + uses: tibdex/github-app-token@v1 + with: + app_id: ${{ secrets.APP_ID }} + private_key: ${{ secrets.PRIVATE_KEY }} + + - uses: actions/setup-node@v3 + with: + node-version: 16 + + - run: npm install @holiday-jp/holiday_jp + + - uses: actions/github-script@v6 + id: is-holiday + with: + script: | + const holiday_jp = require(`${process.env.GITHUB_WORKSPACE}/node_modules/@holiday-jp/holiday_jp`) + core.setOutput('holiday', holiday_jp.isHoliday(new Date())); + - name: Print warning for invalid branch name + if: ${{ inputs.target_branch == 'tier4/main' }} + run: | + echo This action cannot be performed on 'tier4/main' branch + + - name: Run sync-branches + if: ${{ inputs.target_branch != 'tier4/main' }} + uses: autowarefoundation/autoware-github-actions/sync-branches@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + base-branch: ${{ inputs.target_branch }} + sync-pr-branch: sync-upstream + sync-target-repository: https://github.com/autowarefoundation/autoware.universe.git + sync-target-branch: main + pr-title: "chore: sync upstream" + auto-merge-method: merge diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index 04bcfbacbbc17..d986d0f23fc85 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #define EIGEN_MPL2_ONLY #include @@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLIN tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT +BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d) // NOLINT #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index 74c2a60df3f32..f41048169bbd6 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -315,6 +315,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = velocity_texts_.at(i); + delete text; } velocity_texts_.resize(msg_ptr->points.size()); velocity_text_nodes_.resize(msg_ptr->points.size()); @@ -338,6 +341,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); node->removeAndDestroyAllChildren(); this->scene_manager_->destroySceneNode(node); + + rviz_rendering::MovableText * text = slope_texts_.at(i); + delete text; } slope_texts_.resize(msg_ptr->points.size()); slope_text_nodes_.resize(msg_ptr->points.size()); diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index 61c2bd378dab1..51e132b0e7dd5 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -74,7 +74,7 @@ void insertNewlines(std::string & str, const size_t max_line_length) std::optional generateMrmMessage(diagnostic_msgs::msg::DiagnosticStatus diag_status) { - if (diag_status.hardware_id == "" || diag_status.hardware_id == "system_error_monitor") { + if (diag_status.hardware_id == "system_error_monitor") { return std::nullopt; } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { std::string msg = "- ERROR: " + diag_status.name + " (" + diag_status.message + ")"; diff --git a/control/control_validator/config/control_validator.param.yaml b/control/control_validator/config/control_validator.param.yaml index 7bbe6a466799b..54cc0f3f9d712 100644 --- a/control/control_validator/config/control_validator.param.yaml +++ b/control/control_validator/config/control_validator.param.yaml @@ -12,3 +12,4 @@ thresholds: max_distance_deviation: 1.0 + min_velocity_for_checking: 1.0 # m/s diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index 48b7eba7412a2..40f636d6122c1 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -41,6 +41,7 @@ using nav_msgs::msg::Odometry; struct ValidationParams { double max_distance_deviation_threshold; + double min_velocity_for_checking; }; class ControlValidator : public rclcpp::Node diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index ffce38f009a41..f6d49de4c1386 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -61,6 +61,7 @@ void ControlValidator::setupParameters() auto & p = validation_params_; const std::string t = "thresholds."; p.max_distance_deviation_threshold = declare_parameter(t + "max_distance_deviation"); + p.min_velocity_for_checking = declare_parameter(t + "min_velocity_for_checking"); } try { @@ -175,6 +176,10 @@ void ControlValidator::validate(const Trajectory & predicted_trajectory) bool ControlValidator::checkValidMaxDistanceDeviation(const Trajectory & predicted_trajectory) { + if (current_kinematics_->twist.twist.linear.x < validation_params_.min_velocity_for_checking) { + return true; + } + validation_status_.max_distance_deviation = calcMaxLateralDistance(*current_reference_trajectory_, predicted_trajectory); if ( 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 dc55576640133..c658cf4497973 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,14 +29,20 @@ #include #include +#include +#include #include -#include #include +#include +#include +#include +#include #include #include #include +#include #include namespace lane_departure_checker @@ -112,6 +118,19 @@ class LaneDepartureChecker bool checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; + std::vector> getLaneletsFromPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + std::optional getFusedLaneletPolygonForPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + bool checkPathWillLeaveLane( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + PathWithLaneId cropPointsOutsideOfLanes( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, + const size_t end_index); + static bool isOutOfLane( const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint); 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 5bd0fcace9909..c48383a17ab4b 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 @@ -34,6 +34,7 @@ using motion_utils::calcArcLength; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; using tier4_autoware_utils::Point2d; namespace @@ -92,6 +93,7 @@ lanelet::ConstLanelets getCandidateLanelets( return candidate_lanelets; } + } // namespace namespace lane_departure_checker @@ -298,6 +300,98 @@ bool LaneDepartureChecker::willLeaveLane( return false; } +std::vector> LaneDepartureChecker::getLaneletsFromPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + // Get Footprint Hull basic polygon + std::vector vehicle_footprints = createVehicleFootprints(path); + LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); + auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d { + lanelet::BasicPolygon2d basic_polygon; + for (const auto & point : footprint_hull) { + Eigen::Vector2d p(point.x(), point.y()); + basic_polygon.push_back(p); + } + return basic_polygon; + }; + lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull); + + // Find all lanelets that intersect the footprint hull + return lanelet::geometry::findWithin2d( + lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); +} + +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); + auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { + tier4_autoware_utils::Polygon2d p; + auto & outer = p.outer(); + + for (const auto & p : poly) { + tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + outer.push_back(p2d); + } + boost::geometry::correct(p); + return p; + }; + + // Fuse lanelets into a single BasicPolygon2d + auto fused_lanelets = [&]() -> std::optional { + if (lanelets_distance_pair.empty()) return std::nullopt; + tier4_autoware_utils::MultiPolygon2d lanelet_unions; + tier4_autoware_utils::MultiPolygon2d result; + + for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { + const auto & route_lanelet = lanelets_distance_pair.at(i).second; + const auto & p = route_lanelet.polygon2d().basicPolygon(); + tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + boost::geometry::union_(lanelet_unions, poly, result); + lanelet_unions = result; + result.clear(); + } + + if (lanelet_unions.empty()) return std::nullopt; + return lanelet_unions.front(); + }(); + + return fused_lanelets; +} + +bool LaneDepartureChecker::checkPathWillLeaveLane( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + // check if the footprint is not fully contained within the fused lanelets polygon + const std::vector vehicle_footprints = createVehicleFootprints(path); + const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); + if (!fused_lanelets_polygon) return true; + return !std::all_of( + vehicle_footprints.begin(), vehicle_footprints.end(), + [&fused_lanelets_polygon](const auto & footprint) { + return boost::geometry::within(footprint, fused_lanelets_polygon.value()); + }); +} + +PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index) +{ + PathWithLaneId temp_path; + const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); + if (path.points.empty() || !fused_lanelets_polygon) return temp_path; + const auto vehicle_footprints = createVehicleFootprints(path); + size_t idx = 0; + std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { + if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { + temp_path.points.push_back(path.points.at(idx)); + } + ++idx; + }); + PathWithLaneId cropped_path = path; + cropped_path.points = temp_path.points; + return cropped_path; +} + bool LaneDepartureChecker::isOutOfLane( const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint) { @@ -364,4 +458,5 @@ bool LaneDepartureChecker::willCrossBoundary( } return false; } + } // namespace lane_departure_checker diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 06718a2cf0ffa..46bf81c66ddb1 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -410,7 +410,7 @@ def add_launch_arg(name: str, default_value=None, description=None): # component add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") - add_launch_arg("use_multithread", "false", "use multithread") + add_launch_arg("use_multithread", "true", "use multithread") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 227aac50d6d90..10d1ac034d457 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -12,9 +12,6 @@ - - - @@ -113,6 +110,7 @@ + @@ -167,9 +165,7 @@ - - - + @@ -305,6 +301,7 @@ + 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 65caad54876ff..c227298c932d9 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 @@ -139,6 +139,7 @@ + @@ -193,9 +194,7 @@ - - - + @@ -344,6 +343,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 0649d8c5d5116..68d5ea944e6cb 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 @@ -34,11 +34,6 @@ - - - - - @@ -65,14 +60,14 @@ - - - + + + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index fecdd29bcb7e9..a282ed173cf79 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -434,17 +434,17 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con def get_additional_lidars_concatenated_component(input_topics, output_topic): return ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concatenate_data", + plugin="pointcloud_preprocessor::PointCloudConcatenationComponent", + name="concatenate_ground", remappings=[ - ("~/input/odom", "/localization/kinematic_state"), ("output", output_topic), ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), - "input_twist_topic_type": "odom", + "approximate_sync": True, + "timeout_sec": 0.095, } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 7f5f9d61c4bf0..3466596ff1880 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -10,6 +10,9 @@ + + + @@ -94,11 +97,6 @@ description="options: `traffic_light_classifier_mobilenetv2_batch_*` or `traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6" /> - - - - - @@ -109,6 +107,25 @@ description="if use_radar_tracking_fusion:=true, radar information is merged in tracking launch. Otherwise, radar information is merged in detection launch." /> + + + + + + + + + + + + + + + + + + + @@ -122,7 +139,7 @@ - + @@ -131,7 +148,7 @@ - + @@ -152,7 +169,7 @@ - + @@ -172,6 +189,9 @@ + + + @@ -185,9 +205,6 @@ - - - diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index bf81125286256..710fb20631a45 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -7,6 +7,9 @@ + + + @@ -21,7 +24,10 @@ - + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 73e466afc0bda..1ec74793b741b 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -1,4 +1,7 @@ + + + @@ -8,6 +11,8 @@ + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index b36b40ed6a980..53869ec619154 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -4,6 +4,8 @@ + + @@ -192,7 +194,7 @@ - + @@ -200,6 +202,8 @@ + + @@ -226,6 +230,7 @@ + @@ -240,6 +245,7 @@ + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 18de04fd9e317..ece5440bd06bd 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -125,6 +125,7 @@ + @@ -165,6 +166,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 66c90ef2ff833..0a30204ca3c99 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -1,4 +1,6 @@ + + @@ -50,7 +52,10 @@ - + + + + diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 2de6a61547498..3c7d15149061f 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -6,10 +6,7 @@ - - - - + @@ -22,9 +19,16 @@ + + + + + + + @@ -65,7 +69,7 @@ - + @@ -79,7 +83,7 @@ - + @@ -102,6 +106,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -115,4 +155,9 @@ + + + + + diff --git a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml index 0493e1ae0bbcf..0e1d22bfd1827 100644 --- a/launch/tier4_vehicle_launch/launch/vehicle.launch.xml +++ b/launch/tier4_vehicle_launch/launch/vehicle.launch.xml @@ -6,6 +6,7 @@ + @@ -21,6 +22,7 @@ + diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index e14defcb47f26..d59a03d5794e2 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -96,8 +96,10 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::ConstLanelets crosswalk_lanelets = lanelet::utils::query::crosswalkLanelets(all_lanelets); lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); - lanelet::ConstLineStrings3d pedestrian_markings = - lanelet::utils::query::getAllPedestrianMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_polygon_markings = + lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map); + lanelet::ConstLineStrings3d pedestrian_line_markings = + lanelet::utils::query::getAllPedestrianLineMarkings(viz_lanelet_map); lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); std::vector stop_lines = lanelet::utils::query::stopLinesLanelets(road_lanelets); @@ -179,8 +181,12 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); insertMarkerArray( - &map_marker_array, lanelet::visualization::pedestrianMarkingsAsMarkerArray( - pedestrian_markings, cl_pedestrian_markings)); + &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( + pedestrian_polygon_markings, cl_pedestrian_markings)); + + insertMarkerArray( + &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( + pedestrian_line_markings, cl_pedestrian_markings)); insertMarkerArray( &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( "walkway_lanelets", walkway_lanelets, cl_cross)); @@ -215,6 +221,12 @@ void Lanelet2MapVisualizationNode::onMapBin( insertMarkerArray( &map_marker_array, lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + road_lanelets, cl_trafficlights)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( + crosswalk_lanelets, cl_trafficlights)); insertMarkerArray( &map_marker_array, lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index 06bd89af38259..9b5db1372872d 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -95,16 +95,6 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels( distance_threshold, map, voxel)) { return true; } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z - distance_threshold_z), point, distance_threshold, - map, voxel)) { - return true; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z + distance_threshold_z), point, distance_threshold, - map, voxel)) { - return true; - } if (is_in_voxel( pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold_z), point, distance_threshold, map, voxel)) { @@ -235,14 +225,17 @@ bool VoxelGridMapLoader::is_in_voxel( const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, const double distance_threshold, const PointCloudPtr & map, VoxelGridPointXYZ & voxel) const { + const double z_distance_threshold = distance_threshold * downsize_ratio_z_axis_; int voxel_index = voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z)); if (voxel_index != -1) { // not empty voxel const double dist_x = map->points.at(voxel_index).x - target_point.x; const double dist_y = map->points.at(voxel_index).y - target_point.y; - const double dist_z = map->points.at(voxel_index).z - target_point.z; - const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; - if (sqr_distance < distance_threshold * distance_threshold * downsize_ratio_z_axis_) { + const double dist_z = map->points.at(voxel_index).z - target_point.z - z_distance_threshold; + // check if the point is inside the distance threshold voxel + if ( + std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold && + std::abs(dist_z) < z_distance_threshold) { return true; } } diff --git a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml index dfdee95642fed..99050d9738ae6 100644 --- a/perception/detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/detected_object_validation/config/object_lanelet_filter.param.yaml @@ -9,3 +9,13 @@ MOTORCYCLE : false BICYCLE : false PEDESTRIAN : false + + filter_settings: + # polygon overlap based filter + polygon_overlap_filter: + enabled: true + # velocity direction based filter + lanelet_direction_filter: + enabled: false + velocity_yaw_threshold: 0.785398 # [rad] (45 deg) + object_speed_threshold: 3.0 # [m/s] 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 f7a27a52bfa8a..cddee782e8af0 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 @@ -13,3 +13,4 @@ [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] using_2d_validator: false + enable_debugger: false diff --git a/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml new file mode 100644 index 0000000000000..fc5c634735e23 --- /dev/null +++ b/perception/detected_object_validation/config/occupancy_grid_based_validator.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + enable_debug: false diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 4331b5c19d3f1..8c70f020dabd9 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp @@ -17,8 +17,10 @@ #include "detected_object_validation/utils/utils.hpp" +#include #include #include +#include #include #include @@ -27,6 +29,7 @@ #include #include +#include #include namespace object_lanelet_filter @@ -48,6 +51,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node rclcpp::Publisher::SharedPtr object_pub_; rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr object_sub_; + std::unique_ptr debug_publisher_{nullptr}; lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; @@ -58,11 +62,30 @@ class ObjectLaneletFilterNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; utils::FilterTargetLabel filter_target_; - + struct FilterSettings + { + bool polygon_overlap_filter; + bool lanelet_direction_filter; + double lanelet_direction_filter_velocity_yaw_threshold; + double lanelet_direction_filter_object_speed_threshold; + } filter_settings_; + + bool filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &); lanelet::ConstLanelets getIntersectedLanelets( const LinearRing2d &, const lanelet::ConstLanelets &); + bool isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets); bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + bool isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object); geometry_msgs::msg::Polygon setFootprint( const autoware_auto_perception_msgs::msg::DetectedObject &); }; diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp index be25eb6a353e6..059f97cbad431 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_position_filter.hpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace object_position_filter diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 799b605658345..a1d941e66db4b 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -9,7 +9,6 @@ - diff --git a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml index 04bcbd87172b3..3acb1f2d1907a 100644 --- a/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/occupancy_grid_based_validator.launch.xml @@ -7,6 +7,7 @@ + @@ -22,6 +23,6 @@ - + diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index 31bb633294748..f18f8ab922c49 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -6,7 +6,9 @@ The ROS 2 detected_object_validation package Yukihiro Saito Shunsuke Miura - badai nguyen + Dai Nguyen + Shintaro Tomie + Yoshi RI Apache License 2.0 ament_cmake_auto diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index cd16d414425cb..46af50c311013 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -43,6 +43,15 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_target_.MOTORCYCLE = declare_parameter("filter_target_label.MOTORCYCLE", false); filter_target_.BICYCLE = declare_parameter("filter_target_label.BICYCLE", false); filter_target_.PEDESTRIAN = declare_parameter("filter_target_label.PEDESTRIAN", false); + // Set filter settings + filter_settings_.polygon_overlap_filter = + declare_parameter("filter_settings.polygon_overlap_filter.enabled"); + filter_settings_.lanelet_direction_filter = + declare_parameter("filter_settings.lanelet_direction_filter.enabled"); + filter_settings_.lanelet_direction_filter_velocity_yaw_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.velocity_yaw_threshold"); + filter_settings_.lanelet_direction_filter_object_speed_threshold = + declare_parameter("filter_settings.lanelet_direction_filter.object_speed_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -52,6 +61,9 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod "input/object", rclcpp::QoS{1}, std::bind(&ObjectLaneletFilterNode::objectCallback, this, _1)); object_pub_ = this->create_publisher( "output/object", rclcpp::QoS{1}); + + debug_publisher_ = + std::make_unique(this, "object_lanelet_filter"); } void ObjectLaneletFilterNode::mapCallback( @@ -93,29 +105,65 @@ void ObjectLaneletFilterNode::objectCallback( lanelet::ConstLanelets intersected_shoulder_lanelets = getIntersectedLanelets(convex_hull, shoulder_lanelets_); - int index = 0; - for (const auto & object : transformed_objects.objects) { - const auto footprint = setFootprint(object); - const auto & label = object.classification.front().label; - if (filter_target_.isTarget(label)) { - Polygon2d polygon; - for (const auto & point : footprint.points) { - const geometry_msgs::msg::Point32 point_transformed = - tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); - polygon.outer().emplace_back(point_transformed.x, point_transformed.y); - } - polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - } else { - output_object_msg.objects.emplace_back(input_msg->objects.at(index)); - } - ++index; + // filtering process + for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { + const auto & transformed_object = transformed_objects.objects.at(index); + const auto & input_object = input_msg->objects.at(index); + filterObject( + transformed_object, input_object, intersected_road_lanelets, intersected_shoulder_lanelets, + output_object_msg); } object_pub_->publish(output_object_msg); + + // Publish debug info + const double pipeline_latency = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency); +} + +bool ObjectLaneletFilterNode::filterObject( + const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object, + const autoware_auto_perception_msgs::msg::DetectedObject & input_object, + const lanelet::ConstLanelets & intersected_road_lanelets, + const lanelet::ConstLanelets & intersected_shoulder_lanelets, + autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg) +{ + const auto & label = transformed_object.classification.front().label; + if (filter_target_.isTarget(label)) { + bool filter_pass = true; + // 1. is polygon overlap with road lanelets or shoulder lanelets + if (filter_settings_.polygon_overlap_filter) { + const bool is_polygon_overlap = + isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) || + isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets); + filter_pass = filter_pass && is_polygon_overlap; + } + + // 2. check if objects velocity is the same with the lanelet direction + const bool orientation_not_available = + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { + const bool is_same_direction = + isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) || + isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object); + filter_pass = filter_pass && is_same_direction; + } + + // push back to output object + if (filter_pass) { + output_object_msg.objects.emplace_back(input_object); + return true; + } + } else { + output_object_msg.objects.emplace_back(input_object); + return true; + } + return false; } geometry_msgs::msg::Polygon ObjectLaneletFilterNode::setFootprint( @@ -176,6 +224,39 @@ lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( return intersected_lanelets; } +bool ObjectLaneletFilterNode::isObjectOverlapLanelets( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const lanelet::ConstLanelets & intersected_lanelets) +{ + // if has bounding box, use polygon overlap + if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::POLYGON) { + Polygon2d polygon; + const auto footprint = setFootprint(object); + for (const auto & point : footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + polygon.outer().emplace_back(point_transformed.x, point_transformed.y); + } + polygon.outer().push_back(polygon.outer().front()); + return isPolygonOverlapLanelets(polygon, intersected_lanelets); + } else { + // if object do not have bounding box, check each footprint is inside polygon + for (const auto & point : object.shape.footprint.points) { + const geometry_msgs::msg::Point32 point_transformed = + tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose); + geometry_msgs::msg::Pose point2d; + point2d.position.x = point_transformed.x; + point2d.position.y = point_transformed.y; + for (const auto & lanelet : intersected_lanelets) { + if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) { + return true; + } + } + } + return false; + } +} + bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) { @@ -187,6 +268,42 @@ bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( return false; } +bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( + const lanelet::ConstLanelets & lanelets, + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_velocity_norm = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const double object_velocity_yaw = std::atan2( + object.kinematics.twist_with_covariance.twist.linear.y, + object.kinematics.twist_with_covariance.twist.linear.x) + + object_yaw; + + if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) { + return true; + } + for (const auto & lanelet : lanelets) { + const bool is_in_lanelet = + lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0); + if (!is_in_lanelet) { + continue; + } + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_velocity_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); + + if (abs_norm_delta_yaw < filter_settings_.lanelet_direction_filter_velocity_yaw_threshold) { + return true; + } + } + + return false; +} + } // namespace object_lanelet_filter #include diff --git a/perception/detected_object_validation/test/test_utils.cpp b/perception/detected_object_validation/test/test_utils.cpp new file mode 100644 index 0000000000000..ace4dd10a191f --- /dev/null +++ b/perception/detected_object_validation/test/test_utils.cpp @@ -0,0 +1,67 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "detected_object_validation/utils/utils.hpp" + +#include + +#include + +using AutowareLabel = autoware_auto_perception_msgs::msg::ObjectClassification; + +utils::FilterTargetLabel createFilterTargetAll() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = true; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = true; + filter.BICYCLE = true; + filter.PEDESTRIAN = true; + return filter; +} + +utils::FilterTargetLabel createFilterTargetVehicle() +{ + utils::FilterTargetLabel filter; + filter.UNKNOWN = false; + filter.CAR = true; + filter.TRUCK = true; + filter.BUS = true; + filter.TRAILER = true; + filter.MOTORCYCLE = false; + filter.BICYCLE = false; + filter.PEDESTRIAN = false; + return filter; +} + +TEST(IsTargetTest, AllTarget) +{ + auto filter = createFilterTargetAll(); + auto label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(label)); +} + +TEST(IsTargetTest, VehicleTarget) +{ + auto filter = createFilterTargetVehicle(); + + auto car_label = AutowareLabel::CAR; + EXPECT_TRUE(filter.isTarget(car_label)); + + auto unknown_label = AutowareLabel::UNKNOWN; + EXPECT_FALSE(filter.isTarget(unknown_label)); +} diff --git a/perception/ground_segmentation/CMakeLists.txt b/perception/ground_segmentation/CMakeLists.txt index bdd793896062a..656ffec486c3c 100644 --- a/perception/ground_segmentation/CMakeLists.txt +++ b/perception/ground_segmentation/CMakeLists.txt @@ -89,6 +89,7 @@ if(BUILD_TESTING) target_link_libraries(test_ray_ground_filter ground_segmentation + ${YAML_CPP_LIBRARIES} ) ament_add_ros_isolated_gtest(test_scan_ground_filter diff --git a/perception/ground_segmentation/config/ground_segmentation.param.yaml b/perception/ground_segmentation/config/ground_segmentation.param.yaml index 8d56e12244716..594ef1fe974b2 100644 --- a/perception/ground_segmentation/config/ground_segmentation.param.yaml +++ b/perception/ground_segmentation/config/ground_segmentation.param.yaml @@ -29,3 +29,8 @@ gnd_grid_buffer_size: 4 detection_range_z_max: 2.5 elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true + use_lowest_point: true diff --git a/perception/ground_segmentation/config/ransac_ground_filter.param.yaml b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml new file mode 100644 index 0000000000000..0d4a7d574499c --- /dev/null +++ b/perception/ground_segmentation/config/ransac_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + base_frame: "base_link" + unit_axis: "z" + max_iterations: 1000 + min_trial: 5000 + min_points: 1000 + outlier_threshold: 0.01 + plane_slope_threshold: 10.0 + voxel_size_x: 0.04 + voxel_size_y: 0.04 + voxel_size_z: 0.04 + height_threshold: 0.01 + debug: false diff --git a/perception/ground_segmentation/config/ray_ground_filter.param.yaml b/perception/ground_segmentation/config/ray_ground_filter.param.yaml new file mode 100644 index 0000000000000..5b9e8c06595f7 --- /dev/null +++ b/perception/ground_segmentation/config/ray_ground_filter.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + min_x: -0.01 + max_x: 0.01 + min_y: -0.01 + max_y: 0.01 + use_vehicle_footprint: false + general_max_slope: 8.0 + local_max_slope: 6.0 + initial_max_slope: 3.0 + radial_divider_angle: 1.0 + min_height_threshold: 0.15 + concentric_divider_distance: 0.0 + reclass_distance_threshold: 0.1 diff --git a/perception/ground_segmentation/config/scan_ground_filter.param.yaml b/perception/ground_segmentation/config/scan_ground_filter.param.yaml new file mode 100644 index 0000000000000..35d494a620b19 --- /dev/null +++ b/perception/ground_segmentation/config/scan_ground_filter.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + global_slope_max_angle_deg: 10.0 + local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode + split_points_distance_tolerance: 0.2 + use_virtual_ground_point: True + split_height_distance: 0.2 + non_ground_height_threshold: 0.20 + grid_size_m: 0.1 + grid_mode_switch_radius: 20.0 + gnd_grid_buffer_size: 4 + detection_range_z_max: 2.5 + elevation_grid_mode: true + low_priority_region_x: -20.0 + center_pcl_shift: 0.0 + radial_divider_angle_deg: 1.0 + use_recheck_ground_cluster: true + use_lowest_point: true diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 21df6fa6ea1b9..d4eb9c6f3addf 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -50,6 +50,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | | `elevation_grid_mode` | bool | true | Elevation grid scan mode option | | `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | +| `use_lowest_point` | bool | true | to select lowest point for reference in recheck ground cluster, otherwise select middle point | ## Assumptions / Known limits diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index 980565fe2144b..d97bbaa2118e5 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -16,6 +16,7 @@ #define GROUND_SEGMENTATION__SCAN_GROUND_FILTER_NODELET_HPP_ #include "pointcloud_preprocessor/filter.hpp" +#include "pointcloud_preprocessor/transform_info.hpp" #include @@ -50,7 +51,7 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter // classified point label // (0: not classified, 1: ground, 2: not ground, 3: follow previous point, // 4: unkown(currently not used), 5: virtual ground) - enum class PointLabel { + enum class PointLabel : uint16_t { INIT = 0, GROUND, NON_GROUND, @@ -59,19 +60,15 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter VIRTUAL_GROUND, OUT_OF_RANGE }; - struct PointRef + + struct PointData { - float grid_size; // radius of grid - uint16_t grid_id; // id of grid in vertical - float radius; // cylindrical coords on XY Plane - float theta; // angle deg on XY plane - size_t radial_div; // index of the radial division to which this point belongs to + float radius; // cylindrical coords on XY Plane PointLabel point_state{PointLabel::INIT}; - + uint16_t grid_id; // id of grid in vertical size_t orig_index; // index of this point in the source pointcloud - pcl::PointXYZ * orig_point; }; - using PointCloudRefVector = std::vector; + using PointCloudVector = std::vector; struct GridCenter { @@ -144,18 +141,39 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter pcl::PointIndices getIndices() { return pcl_indices; } std::vector getHeightList() { return height_list; } + + pcl::PointIndices & getIndicesRef() { return pcl_indices; } + std::vector & getHeightListRef() { return height_list; } }; void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + // TODO(taisa1): Temporary Implementation: Remove this interface when all the filter nodes + // conform to new API + virtual void faster_filter( + const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, + const pointcloud_preprocessor::TransformInfo & transform_info); + tf2_ros::Buffer tf_buffer_{get_clock()}; tf2_ros::TransformListener tf_listener_{tf_buffer_}; + int x_offset_; + int y_offset_; + int z_offset_; + int intensity_offset_; + bool offset_initialized_; + + void set_field_offsets(const PointCloud2ConstPtr & input); + + void get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset); + const uint16_t gnd_grid_continual_thresh_ = 3; bool elevation_grid_mode_; float non_ground_height_threshold_; float grid_size_rad_; + float tan_grid_size_rad_; float grid_size_m_; float low_priority_region_x_; uint16_t gnd_grid_buffer_size_; @@ -163,16 +181,21 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter float grid_mode_switch_angle_rad_; float virtual_lidar_z_; float detection_range_z_max_; - float center_pcl_shift_; // virtual center of pcl to center mass - float grid_mode_switch_radius_; // non linear grid size switching distance - double global_slope_max_angle_rad_; // radians - double local_slope_max_angle_rad_; // radians + float center_pcl_shift_; // virtual center of pcl to center mass + float grid_mode_switch_radius_; // non linear grid size switching distance + double global_slope_max_angle_rad_; // radians + double local_slope_max_angle_rad_; // radians + double global_slope_max_ratio_; + double local_slope_max_ratio_; double radial_divider_angle_rad_; // distance in rads between dividers double split_points_distance_tolerance_; // distance in meters between concentric divisions - double // minimum height threshold regardless the slope, - split_height_distance_; // useful for close points + double split_points_distance_tolerance_square_; + double // minimum height threshold regardless the slope, + split_height_distance_; // useful for close points bool use_virtual_ground_point_; bool use_recheck_ground_cluster_; // to enable recheck ground cluster + bool use_lowest_point_; // to select lowest point for reference in recheck ground cluster, + // otherwise select middle point size_t radial_dividers_num_; VehicleInfo vehicle_info_; @@ -186,23 +209,25 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter */ /*! - * Convert pcl::PointCloud to sorted PointCloudRefVector + * Convert sensor_msgs::msg::PointCloud2 to sorted PointCloudVector * @param[in] in_cloud Input Point Cloud to be organized in radial segments * @param[out] out_radial_ordered_points_manager Vector of Points Clouds, * each element will contain the points ordered */ void convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); void convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points_manager); + const PointCloud2ConstPtr & in_cloud, + std::vector & out_radial_ordered_points_manager); /*! * Output ground center of front wheels as the virtual ground point * @param[out] point Virtual ground origin point */ void calcVirtualGroundOrigin(pcl::PointXYZ & point); + float calcGridSize(const PointData & p); + /*! * Classifies Points in the PointCloud as Ground and Not Ground * @param in_radial_ordered_clouds Vector of an Ordered PointsCloud @@ -214,14 +239,19 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter void initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids); - void checkContinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkDiscontinuousGndGrid(PointRef & p, const std::vector & gnd_grids_list); - void checkBreakGndGrid(PointRef & p, const std::vector & gnd_grids_list); + void checkContinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkDiscontinuousGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); + void checkBreakGndGrid( + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list); void classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); void classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud_ptr, + std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices); /*! * Re-classifies point of ground cluster based on their height @@ -230,18 +260,18 @@ class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter * @param non_ground_indices Output non-ground PointCloud indices */ void recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, + PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, pcl::PointIndices & non_ground_indices); /*! * Returns the resulting complementary PointCloud, one with the points kept * and the other removed as indicated in the indices * @param in_cloud_ptr Input PointCloud to which the extraction will be performed * @param in_indices Indices of the points to be both removed and kept - * @param out_object_cloud_ptr Resulting PointCloud with the indices kept + * @param out_object_cloud Resulting PointCloud with the indices kept */ void extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr); + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud); /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml new file mode 100644 index 0000000000000..f8280b774c6b0 --- /dev/null +++ b/perception/ground_segmentation/launch/ransac_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/ray_ground_filter.launch.xml b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml new file mode 100644 index 0000000000000..86375c520a426 --- /dev/null +++ b/perception/ground_segmentation/launch/ray_ground_filter.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml new file mode 100644 index 0000000000000..2a9e4983ecb56 --- /dev/null +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp index ebcb02df2c614..ed5fb6abe7fe7 100644 --- a/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ransac_ground_filter_nodelet.cpp @@ -85,18 +85,18 @@ using pointcloud_preprocessor::get_param; RANSACGroundFilterComponent::RANSACGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("RANSACGroundFilter", options) { - base_frame_ = declare_parameter("base_frame", "base_link"); - unit_axis_ = declare_parameter("unit_axis", "z"); - max_iterations_ = declare_parameter("max_iterations", 1000); - min_inliers_ = declare_parameter("min_trial", 5000); - min_points_ = declare_parameter("min_points", 1000); - outlier_threshold_ = declare_parameter("outlier_threshold", 0.01); - plane_slope_threshold_ = declare_parameter("plane_slope_threshold", 10.0); - voxel_size_x_ = declare_parameter("voxel_size_x", 0.04); - voxel_size_y_ = declare_parameter("voxel_size_y", 0.04); - voxel_size_z_ = declare_parameter("voxel_size_z", 0.04); - height_threshold_ = declare_parameter("height_threshold", 0.01); - debug_ = declare_parameter("debug", false); + base_frame_ = declare_parameter("base_frame", "base_link"); + unit_axis_ = declare_parameter("unit_axis"); + max_iterations_ = declare_parameter("max_iterations"); + min_inliers_ = declare_parameter("min_trial"); + min_points_ = declare_parameter("min_points"); + outlier_threshold_ = declare_parameter("outlier_threshold"); + plane_slope_threshold_ = declare_parameter("plane_slope_threshold"); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); + height_threshold_ = declare_parameter("height_threshold"); + debug_ = declare_parameter("debug"); if (unit_axis_ == "x") { unit_vec_ = Eigen::Vector3d::UnitX(); diff --git a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp index 4b2e81d272e30..7bcae47fd9f1f 100644 --- a/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/ray_ground_filter_nodelet.cpp @@ -48,22 +48,22 @@ RayGroundFilterComponent::RayGroundFilterComponent(const rclcpp::NodeOptions & o grid_precision_ = 0.2; ray_ground_filter::generateColors(colors_, color_num_); - min_x_ = declare_parameter("min_x", -0.01); - max_x_ = declare_parameter("max_x", 0.01); - min_y_ = declare_parameter("min_y", -0.01); - max_y_ = declare_parameter("max_y", 0.01); + min_x_ = declare_parameter("min_x"); + max_x_ = declare_parameter("max_x"); + min_y_ = declare_parameter("min_y"); + max_y_ = declare_parameter("max_y"); setVehicleFootprint(min_x_, max_x_, min_y_, max_y_); - use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); + use_vehicle_footprint_ = declare_parameter("use_vehicle_footprint", false); - general_max_slope_ = declare_parameter("general_max_slope", 8.0); - local_max_slope_ = declare_parameter("local_max_slope", 6.0); - initial_max_slope_ = declare_parameter("initial_max_slope", 3.0); - radial_divider_angle_ = declare_parameter("radial_divider_angle", 1.0); - min_height_threshold_ = declare_parameter("min_height_threshold", 0.15); - concentric_divider_distance_ = declare_parameter("concentric_divider_distance", 0.0); - reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold", 0.1); + general_max_slope_ = declare_parameter("general_max_slope"); + local_max_slope_ = declare_parameter("local_max_slope"); + initial_max_slope_ = declare_parameter("initial_max_slope"); + radial_divider_angle_ = declare_parameter("radial_divider_angle"); + min_height_threshold_ = declare_parameter("min_height_threshold"); + concentric_divider_distance_ = declare_parameter("concentric_divider_distance"); + reclass_distance_threshold_ = declare_parameter("reclass_distance_threshold"); } using std::placeholders::_1; diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index 953a9feb4d21e..34573b564fa36 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -37,24 +37,27 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & { // set initial parameters { - low_priority_region_x_ = static_cast(declare_parameter("low_priority_region_x", -20.0f)); - detection_range_z_max_ = static_cast(declare_parameter("detection_range_z_max", 2.5f)); - center_pcl_shift_ = static_cast(declare_parameter("center_pcl_shift", 0.0)); - non_ground_height_threshold_ = - static_cast(declare_parameter("non_ground_height_threshold", 0.20)); - grid_mode_switch_radius_ = - static_cast(declare_parameter("grid_mode_switch_radius", 20.0)); - - grid_size_m_ = static_cast(declare_parameter("grid_size_m", 0.5)); - gnd_grid_buffer_size_ = static_cast(declare_parameter("gnd_grid_buffer_size", 4)); - elevation_grid_mode_ = static_cast(declare_parameter("elevation_grid_mode", true)); - global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg", 8.0)); - local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg", 10.0)); - radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg", 1.0)); - split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance", 0.2); - split_height_distance_ = declare_parameter("split_height_distance", 0.2); - use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point", true); - use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster", true); + low_priority_region_x_ = declare_parameter("low_priority_region_x"); + detection_range_z_max_ = declare_parameter("detection_range_z_max"); + center_pcl_shift_ = declare_parameter("center_pcl_shift"); + non_ground_height_threshold_ = declare_parameter("non_ground_height_threshold"); + grid_mode_switch_radius_ = declare_parameter("grid_mode_switch_radius"); + + grid_size_m_ = declare_parameter("grid_size_m"); + gnd_grid_buffer_size_ = declare_parameter("gnd_grid_buffer_size"); + elevation_grid_mode_ = declare_parameter("elevation_grid_mode"); + global_slope_max_angle_rad_ = deg2rad(declare_parameter("global_slope_max_angle_deg")); + local_slope_max_angle_rad_ = deg2rad(declare_parameter("local_slope_max_angle_deg")); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); + radial_divider_angle_rad_ = deg2rad(declare_parameter("radial_divider_angle_deg")); + split_points_distance_tolerance_ = declare_parameter("split_points_distance_tolerance"); + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; + split_height_distance_ = declare_parameter("split_height_distance"); + use_virtual_ground_point_ = declare_parameter("use_virtual_ground_point"); + use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); + use_lowest_point_ = declare_parameter("use_lowest_point"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); @@ -62,6 +65,12 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division virtual_lidar_z_ = vehicle_info_.vehicle_height_m; grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + offset_initialized_ = false; } using std::placeholders::_1; @@ -79,92 +88,117 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & } } +inline void ScanGroundFilterComponent::set_field_offsets(const PointCloud2ConstPtr & input) +{ + x_offset_ = input->fields[pcl::getFieldIndex(*input, "x")].offset; + y_offset_ = input->fields[pcl::getFieldIndex(*input, "y")].offset; + z_offset_ = input->fields[pcl::getFieldIndex(*input, "z")].offset; + int intensity_index = pcl::getFieldIndex(*input, "intensity"); + if (intensity_index != -1) { + intensity_offset_ = input->fields[intensity_index].offset; + } else { + intensity_offset_ = z_offset_ + sizeof(float); + } + offset_initialized_ = true; +} + +inline void ScanGroundFilterComponent::get_point_from_global_offset( + const PointCloud2ConstPtr & input, pcl::PointXYZ & point, size_t global_offset) +{ + point.x = *reinterpret_cast(&input->data[global_offset + x_offset_]); + point.y = *reinterpret_cast(&input->data[global_offset + y_offset_]); + point.z = *reinterpret_cast(&input->data[global_offset + z_offset_]); +} + void ScanGroundFilterComponent::convertPointcloudGridScan( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; - uint16_t back_steps_num = 1; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + const auto inv_grid_size_rad = 1.0f / grid_size_rad_; + const auto inv_grid_size_m = 1.0f / grid_size_m_; + + const auto grid_id_offset = + grid_mode_switch_grid_id_ - grid_mode_switch_angle_rad_ * inv_grid_size_rad; + const auto x_shift = vehicle_info_.wheel_base_m / 2.0f + center_pcl_shift_; - grid_size_rad_ = - normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - - normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto x{ - in_cloud->points[i].x - vehicle_info_.wheel_base_m / 2.0f - - center_pcl_shift_}; // base on front wheel center - // auto y{in_cloud->points[i].y}; - auto radius{static_cast(std::hypot(x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(x, in_cloud->points[i].y), 0.0)}; + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; + + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto x{input_point.x - x_shift}; // base on front wheel center + auto radius{static_cast(std::hypot(x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(x, input_point.y), 0.0)}; // divide by vertical angle - auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; uint16_t grid_id = 0; - float curr_grid_size = 0.0f; if (radius <= grid_mode_switch_radius_) { - grid_id = static_cast(radius / grid_size_m_); - curr_grid_size = grid_size_m_; + grid_id = static_cast(radius * inv_grid_size_m); } else { - grid_id = grid_mode_switch_grid_id_ + (gamma - grid_mode_switch_angle_rad_) / grid_size_rad_; - if (grid_id <= grid_mode_switch_grid_id_ + back_steps_num) { - curr_grid_size = grid_size_m_; - } else { - curr_grid_size = std::tan(gamma) - std::tan(gamma - grid_size_rad_); - curr_grid_size *= virtual_lidar_z_; - } + auto gamma{normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f)}; + grid_id = grid_id_offset + gamma * inv_grid_size_rad; } current_point.grid_id = grid_id; - current_point.grid_size = curr_grid_size; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } + void ScanGroundFilterComponent::convertPointcloud( - const pcl::PointCloud::Ptr in_cloud, - std::vector & out_radial_ordered_points) + const PointCloud2ConstPtr & in_cloud, std::vector & out_radial_ordered_points) { out_radial_ordered_points.resize(radial_dividers_num_); - PointRef current_point; + PointData current_point; + + const auto inv_radial_divider_angle_rad = 1.0f / radial_divider_angle_rad_; + + const size_t in_cloud_data_size = in_cloud->data.size(); + const size_t in_cloud_point_step = in_cloud->point_step; - for (size_t i = 0; i < in_cloud->points.size(); ++i) { - auto radius{static_cast(std::hypot(in_cloud->points[i].x, in_cloud->points[i].y))}; - auto theta{normalizeRadian(std::atan2(in_cloud->points[i].x, in_cloud->points[i].y), 0.0)}; - auto radial_div{ - static_cast(std::floor(normalizeDegree(theta / radial_divider_angle_rad_, 0.0)))}; + size_t point_index = 0; + pcl::PointXYZ input_point; + for (size_t global_offset = 0; global_offset + in_cloud_point_step <= in_cloud_data_size; + global_offset += in_cloud_point_step) { + // Point + get_point_from_global_offset(in_cloud, input_point, global_offset); + + auto radius{static_cast(std::hypot(input_point.x, input_point.y))}; + auto theta{normalizeRadian(std::atan2(input_point.x, input_point.y), 0.0)}; + auto radial_div{static_cast(std::floor(theta * inv_radial_divider_angle_rad))}; current_point.radius = radius; - current_point.theta = theta; - current_point.radial_div = radial_div; current_point.point_state = PointLabel::INIT; - current_point.orig_index = i; - current_point.orig_point = &in_cloud->points[i]; + current_point.orig_index = point_index; // radial divisions out_radial_ordered_points[radial_div].emplace_back(current_point); + ++point_index; } - // sort by distance for (size_t i = 0; i < radial_dividers_num_; ++i) { std::sort( out_radial_ordered_points[i].begin(), out_radial_ordered_points[i].end(), - [](const PointRef & a, const PointRef & b) { return a.radius < b.radius; }); + [](const PointData & a, const PointData & b) { return a.radius < b.radius; }); } } @@ -175,6 +209,21 @@ void ScanGroundFilterComponent::calcVirtualGroundOrigin(pcl::PointXYZ & point) point.z = 0; } +inline float ScanGroundFilterComponent::calcGridSize(const PointData & p) +{ + float grid_size = grid_size_m_; + uint16_t back_steps_num = 1; + + if ( + p.radius > grid_mode_switch_radius_ && p.grid_id > grid_mode_switch_grid_id_ + back_steps_num) { + // equivalent to grid_size = (std::tan(gamma) - std::tan(gamma - grid_size_rad_)) * + // virtual_lidar_z_ when gamma = normalizeRadian(std::atan2(radius, virtual_lidar_z_), 0.0f) + grid_size = p.radius - (p.radius - tan_grid_size_rad_ * virtual_lidar_z_) / + (1 + p.radius * tan_grid_size_rad_ / virtual_lidar_z_); + } + return grid_size; +} + void ScanGroundFilterComponent::initializeFirstGndGrids( const float h, const float r, const uint16_t id, std::vector & gnd_grids) { @@ -195,96 +244,95 @@ void ScanGroundFilterComponent::initializeFirstGndGrids( } void ScanGroundFilterComponent::checkContinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { float next_gnd_z = 0.0f; - float curr_gnd_slope_rad = 0.0f; + float curr_gnd_slope_ratio = 0.0f; float gnd_buff_z_mean = 0.0f; - float gnd_buff_z_max = 0.0f; float gnd_buff_radius = 0.0f; for (auto it = gnd_grids_list.end() - gnd_grid_buffer_size_ - 1; it < gnd_grids_list.end() - 1; ++it) { gnd_buff_radius += it->radius; gnd_buff_z_mean += it->avg_height; - gnd_buff_z_max += it->max_height; } gnd_buff_radius /= static_cast(gnd_grid_buffer_size_ - 1); - gnd_buff_z_max /= static_cast(gnd_grid_buffer_size_ - 1); gnd_buff_z_mean /= static_cast(gnd_grid_buffer_size_ - 1); float tmp_delta_mean_z = gnd_grids_list.back().avg_height - gnd_buff_z_mean; float tmp_delta_radius = gnd_grids_list.back().radius - gnd_buff_radius; - curr_gnd_slope_rad = std::atan(tmp_delta_mean_z / tmp_delta_radius); - curr_gnd_slope_rad = curr_gnd_slope_rad < -global_slope_max_angle_rad_ - ? -global_slope_max_angle_rad_ - : curr_gnd_slope_rad; - curr_gnd_slope_rad = curr_gnd_slope_rad > global_slope_max_angle_rad_ - ? global_slope_max_angle_rad_ - : curr_gnd_slope_rad; + curr_gnd_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; + curr_gnd_slope_ratio = curr_gnd_slope_ratio < -global_slope_max_ratio_ ? -global_slope_max_ratio_ + : curr_gnd_slope_ratio; + curr_gnd_slope_ratio = + curr_gnd_slope_ratio > global_slope_max_ratio_ ? global_slope_max_ratio_ : curr_gnd_slope_ratio; - next_gnd_z = std::tan(curr_gnd_slope_rad) * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; + next_gnd_z = curr_gnd_slope_ratio * (p.radius - gnd_buff_radius) + gnd_buff_z_mean; float gnd_z_local_thresh = std::tan(DEG2RAD(5.0)) * (p.radius - gnd_grids_list.back().radius); - tmp_delta_mean_z = p.orig_point->z - (gnd_grids_list.end() - 2)->avg_height; + tmp_delta_mean_z = p_orig_point.z - (gnd_grids_list.end() - 2)->avg_height; tmp_delta_radius = p.radius - (gnd_grids_list.end() - 2)->radius; - float local_slope = std::atan(tmp_delta_mean_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_mean_z / tmp_delta_radius; if ( - abs(p.orig_point->z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || - abs(local_slope) <= local_slope_max_angle_rad_) { + abs(p_orig_point.z - next_gnd_z) <= non_ground_height_threshold_ + gnd_z_local_thresh || + abs(local_slope_ratio) <= local_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (p.orig_point->z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { + } else if (p_orig_point.z - next_gnd_z > non_ground_height_threshold_ + gnd_z_local_thresh) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::checkDiscontinuousGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_max_z = p.orig_point->z - gnd_grids_list.back().max_height; - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_max_z = p_orig_point.z - gnd_grids_list.back().max_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; if ( - abs(local_slope) < local_slope_max_angle_rad_ || + abs(local_slope_ratio) < local_slope_max_ratio_ || abs(tmp_delta_avg_z) < non_ground_height_threshold_ || abs(tmp_delta_max_z) < non_ground_height_threshold_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } void ScanGroundFilterComponent::checkBreakGndGrid( - PointRef & p, const std::vector & gnd_grids_list) + PointData & p, pcl::PointXYZ & p_orig_point, const std::vector & gnd_grids_list) { - float tmp_delta_avg_z = p.orig_point->z - gnd_grids_list.back().avg_height; + float tmp_delta_avg_z = p_orig_point.z - gnd_grids_list.back().avg_height; float tmp_delta_radius = p.radius - gnd_grids_list.back().radius; - float local_slope = std::atan(tmp_delta_avg_z / tmp_delta_radius); - if (abs(local_slope) < global_slope_max_angle_rad_) { + float local_slope_ratio = tmp_delta_avg_z / tmp_delta_radius; + if (abs(local_slope_ratio) < global_slope_max_ratio_) { p.point_state = PointLabel::GROUND; - } else if (local_slope > global_slope_max_angle_rad_) { + } else if (local_slope_ratio > global_slope_max_ratio_) { p.point_state = PointLabel::NON_GROUND; } } + void ScanGroundFilterComponent::recheckGroundCluster( - PointsCentroid & gnd_cluster, const float non_ground_threshold, + PointsCentroid & gnd_cluster, const float non_ground_threshold, const bool use_lowest_point, pcl::PointIndices & non_ground_indices) { - const float min_gnd_height = gnd_cluster.getMinHeight(); - pcl::PointIndices gnd_indices = gnd_cluster.getIndices(); - std::vector height_list = gnd_cluster.getHeightList(); + float reference_height = + use_lowest_point ? gnd_cluster.getMinHeight() : gnd_cluster.getAverageHeight(); + const pcl::PointIndices & gnd_indices = gnd_cluster.getIndicesRef(); + const std::vector & height_list = gnd_cluster.getHeightListRef(); for (size_t i = 0; i < height_list.size(); ++i) { - if (height_list.at(i) >= min_gnd_height + non_ground_threshold) { + if (height_list.at(i) >= reference_height + non_ground_threshold) { non_ground_indices.indices.push_back(gnd_indices.indices.at(i)); } } } + void ScanGroundFilterComponent::classifyPointCloudGridScan( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -301,42 +349,41 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( // check the first point in ray auto * p = &in_radial_ordered_clouds[i][0]; - PointRef * prev_p; - prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point + auto * prev_p = &in_radial_ordered_clouds[i][0]; // for checking the distance to prev point bool initialized_first_gnd_grid = false; bool prev_list_init = false; - - for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - p = &in_radial_ordered_clouds[i][j]; - float global_slope_p = std::atan(p->orig_point->z / p->radius); + pcl::PointXYZ p_orig_point, prev_p_orig_point; + for (auto & point : in_radial_ordered_clouds[i]) { + prev_p = p; + prev_p_orig_point = p_orig_point; + p = &point; + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); + float global_slope_ratio_p = p_orig_point.z / p->radius; float non_ground_height_threshold_local = non_ground_height_threshold_; - if (p->orig_point->x < low_priority_region_x_) { + if (p_orig_point.x < low_priority_region_x_) { non_ground_height_threshold_local = - non_ground_height_threshold_ * abs(p->orig_point->x / low_priority_region_x_); + non_ground_height_threshold_ * abs(p_orig_point.x / low_priority_region_x_); } // classify first grid's point cloud if ( - !initialized_first_gnd_grid && global_slope_p >= global_slope_max_angle_rad_ && - p->orig_point->z > non_ground_height_threshold_local) { + !initialized_first_gnd_grid && global_slope_ratio_p >= global_slope_max_ratio_ && + p_orig_point.z > non_ground_height_threshold_local) { out_no_ground_indices.indices.push_back(p->orig_index); p->point_state = PointLabel::NON_GROUND; - prev_p = p; continue; } if ( - !initialized_first_gnd_grid && abs(global_slope_p) < global_slope_max_angle_rad_ && - abs(p->orig_point->z) < non_ground_height_threshold_local) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + !initialized_first_gnd_grid && abs(global_slope_ratio_p) < global_slope_max_ratio_ && + abs(p_orig_point.z) < non_ground_height_threshold_local) { + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); p->point_state = PointLabel::GROUND; initialized_first_gnd_grid = static_cast(p->grid_id - prev_p->grid_id); - prev_p = p; continue; } if (!initialized_first_gnd_grid) { - prev_p = p; continue; } @@ -358,7 +405,8 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( if (p->grid_id > prev_p->grid_id && ground_cluster.getAverageRadius() > 0.0) { // check if the prev grid have ground point cloud if (use_recheck_ground_cluster_) { - recheckGroundCluster(ground_cluster, non_ground_height_threshold_, out_no_ground_indices); + recheckGroundCluster( + ground_cluster, non_ground_height_threshold_, use_lowest_point_, out_no_ground_indices); } curr_gnd_grid.radius = ground_cluster.getAverageRadius(); curr_gnd_grid.avg_height = ground_cluster.getAverageHeight(); @@ -368,53 +416,50 @@ void ScanGroundFilterComponent::classifyPointCloudGridScan( ground_cluster.initialize(); } // classify - if (p->orig_point->z - gnd_grids.back().avg_height > detection_range_z_max_) { + if (p_orig_point.z - gnd_grids.back().avg_height > detection_range_z_max_) { p->point_state = PointLabel::OUT_OF_RANGE; - prev_p = p; continue; } - float points_xy_distance = std::hypot( - p->orig_point->x - prev_p->orig_point->x, p->orig_point->y - prev_p->orig_point->y); + float points_xy_distance_square = + (p_orig_point.x - prev_p_orig_point.x) * (p_orig_point.x - prev_p_orig_point.x) + + (p_orig_point.y - prev_p_orig_point.y) * (p_orig_point.y - prev_p_orig_point.y); if ( prev_p->point_state == PointLabel::NON_GROUND && - points_xy_distance < split_points_distance_tolerance_ && - p->orig_point->z > prev_p->orig_point->z) { + points_xy_distance_square < split_points_distance_tolerance_square_ && + p_orig_point.z > prev_p_orig_point.z) { p->point_state = PointLabel::NON_GROUND; out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } - - if (global_slope_p > global_slope_max_angle_rad_) { + if (global_slope_ratio_p > global_slope_max_ratio_) { out_no_ground_indices.indices.push_back(p->orig_index); - prev_p = p; continue; } // gnd grid is continuous, the last gnd grid is close uint16_t next_gnd_grid_id_thresh = (gnd_grids.end() - gnd_grid_buffer_size_)->grid_id + gnd_grid_buffer_size_ + gnd_grid_continual_thresh_; + float curr_grid_size = calcGridSize(*p); if ( p->grid_id < next_gnd_grid_id_thresh && - p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkContinuousGndGrid(*p, gnd_grids); - - } else if (p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * p->grid_size) { - checkDiscontinuousGndGrid(*p, gnd_grids); + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkContinuousGndGrid(*p, p_orig_point, gnd_grids); + } else if ( + p->radius - gnd_grids.back().radius < gnd_grid_continual_thresh_ * curr_grid_size) { + checkDiscontinuousGndGrid(*p, p_orig_point, gnd_grids); } else { - checkBreakGndGrid(*p, gnd_grids); + checkBreakGndGrid(*p, p_orig_point, gnd_grids); } if (p->point_state == PointLabel::NON_GROUND) { out_no_ground_indices.indices.push_back(p->orig_index); } else if (p->point_state == PointLabel::GROUND) { - ground_cluster.addPoint(p->radius, p->orig_point->z, p->orig_index); + ground_cluster.addPoint(p->radius, p_orig_point.z, p->orig_index); } - prev_p = p; } } } void ScanGroundFilterComponent::classifyPointCloud( - std::vector & in_radial_ordered_clouds, + const PointCloud2ConstPtr & in_cloud, std::vector & in_radial_ordered_clouds, pcl::PointIndices & out_no_ground_indices) { out_no_ground_indices.indices.clear(); @@ -432,16 +477,15 @@ void ScanGroundFilterComponent::classifyPointCloud( PointsCentroid ground_cluster, non_ground_cluster; float local_slope = 0.0f; PointLabel prev_point_label = PointLabel::INIT; - pcl::PointXYZ prev_gnd_point(0, 0, 0); + pcl::PointXYZ prev_gnd_point(0, 0, 0), p_orig_point, prev_p_orig_point; // loop through each point in the radial div for (size_t j = 0; j < in_radial_ordered_clouds[i].size(); ++j) { - const float global_slope_max_angle = global_slope_max_angle_rad_; const float local_slope_max_angle = local_slope_max_angle_rad_; + prev_p_orig_point = p_orig_point; auto * p = &in_radial_ordered_clouds[i][j]; - auto * p_prev = &in_radial_ordered_clouds[i][j - 1]; - + get_point_from_global_offset(in_cloud, p_orig_point, in_cloud->point_step * p->orig_index); if (j == 0) { - bool is_front_side = (p->orig_point->x > virtual_ground_point.x); + bool is_front_side = (p_orig_point.x > virtual_ground_point.x); if (use_virtual_ground_point_ && is_front_side) { prev_gnd_point = virtual_ground_point; } else { @@ -451,22 +495,22 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_gnd_slope = 0.0f; ground_cluster.initialize(); non_ground_cluster.initialize(); - points_distance = calcDistance3d(*p->orig_point, prev_gnd_point); + points_distance = calcDistance3d(p_orig_point, prev_gnd_point); } else { - points_distance = calcDistance3d(*p->orig_point, *p_prev->orig_point); + points_distance = calcDistance3d(p_orig_point, prev_p_orig_point); } float radius_distance_from_gnd = p->radius - prev_gnd_radius; - float height_from_gnd = p->orig_point->z - prev_gnd_point.z; - float height_from_obj = p->orig_point->z - non_ground_cluster.getAverageHeight(); + float height_from_gnd = p_orig_point.z - prev_gnd_point.z; + float height_from_obj = p_orig_point.z - non_ground_cluster.getAverageHeight(); bool calculate_slope = false; bool is_point_close_to_prev = (points_distance < (p->radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); - float global_slope = std::atan2(p->orig_point->z, p->radius); + float global_slope_ratio = p_orig_point.z / p->radius; // check points which is far enough from previous point - if (global_slope > global_slope_max_angle) { + if (global_slope_ratio > global_slope_max_ratio_) { p->point_state = PointLabel::NON_GROUND; calculate_slope = false; } else if ( @@ -481,7 +525,7 @@ void ScanGroundFilterComponent::classifyPointCloud( calculate_slope = true; } if (is_point_close_to_prev) { - height_from_gnd = p->orig_point->z - ground_cluster.getAverageHeight(); + height_from_gnd = p_orig_point.z - ground_cluster.getAverageHeight(); radius_distance_from_gnd = p->radius - ground_cluster.getAverageRadius(); } if (calculate_slope) { @@ -516,58 +560,66 @@ void ScanGroundFilterComponent::classifyPointCloud( prev_point_label = p->point_state; if (p->point_state == PointLabel::GROUND) { prev_gnd_radius = p->radius; - prev_gnd_point = pcl::PointXYZ(p->orig_point->x, p->orig_point->y, p->orig_point->z); - ground_cluster.addPoint(p->radius, p->orig_point->z); + prev_gnd_point = pcl::PointXYZ(p_orig_point.x, p_orig_point.y, p_orig_point.z); + ground_cluster.addPoint(p->radius, p_orig_point.z); prev_gnd_slope = ground_cluster.getAverageSlope(); } // update the non ground state if (p->point_state == PointLabel::NON_GROUND) { - non_ground_cluster.addPoint(p->radius, p->orig_point->z); + non_ground_cluster.addPoint(p->radius, p_orig_point.z); } } } } void ScanGroundFilterComponent::extractObjectPoints( - const pcl::PointCloud::Ptr in_cloud_ptr, const pcl::PointIndices & in_indices, - pcl::PointCloud::Ptr out_object_cloud_ptr) + const PointCloud2ConstPtr & in_cloud_ptr, const pcl::PointIndices & in_indices, + PointCloud2 & out_object_cloud) { + size_t output_data_size = 0; for (const auto & i : in_indices.indices) { - out_object_cloud_ptr->points.emplace_back(in_cloud_ptr->points[i]); + std::memcpy( + &out_object_cloud.data[output_data_size], &in_cloud_ptr->data[i * in_cloud_ptr->point_step], + in_cloud_ptr->point_step * sizeof(uint8_t)); + *reinterpret_cast(&out_object_cloud.data[output_data_size + intensity_offset_]) = + 1; // set intensity to 1 + output_data_size += in_cloud_ptr->point_step; } } -void ScanGroundFilterComponent::filter( +void ScanGroundFilterComponent::faster_filter( const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, - PointCloud2 & output) + PointCloud2 & output, + [[maybe_unused]] const pointcloud_preprocessor::TransformInfo & transform_info) { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - pcl::PointCloud::Ptr current_sensor_cloud_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*input, *current_sensor_cloud_ptr); - std::vector radial_ordered_points; + if (!offset_initialized_) { + set_field_offsets(input); + } + std::vector radial_ordered_points; pcl::PointIndices no_ground_indices; - pcl::PointCloud::Ptr no_ground_cloud_ptr(new pcl::PointCloud); - no_ground_cloud_ptr->points.reserve(current_sensor_cloud_ptr->points.size()); if (elevation_grid_mode_) { - convertPointcloudGridScan(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloudGridScan(radial_ordered_points, no_ground_indices); + convertPointcloudGridScan(input, radial_ordered_points); + classifyPointCloudGridScan(input, radial_ordered_points, no_ground_indices); } else { - convertPointcloud(current_sensor_cloud_ptr, radial_ordered_points); - classifyPointCloud(radial_ordered_points, no_ground_indices); + convertPointcloud(input, radial_ordered_points); + classifyPointCloud(input, radial_ordered_points, no_ground_indices); } - - extractObjectPoints(current_sensor_cloud_ptr, no_ground_indices, no_ground_cloud_ptr); - - auto no_ground_cloud_msg_ptr = std::make_shared(); - pcl::toROSMsg(*no_ground_cloud_ptr, *no_ground_cloud_msg_ptr); - - no_ground_cloud_msg_ptr->header = input->header; - output = *no_ground_cloud_msg_ptr; - + output.row_step = no_ground_indices.indices.size() * input->point_step; + output.data.resize(output.row_step); + output.width = no_ground_indices.indices.size(); + output.fields.assign(input->fields.begin(), input->fields.begin() + 3); + output.is_dense = true; + output.height = input->height; + output.is_bigendian = input->is_bigendian; + output.point_step = input->point_step; + output.header = input->header; + + extractObjectPoints(input, no_ground_indices, output); if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); @@ -578,20 +630,62 @@ void ScanGroundFilterComponent::filter( } } +// TODO(taisa1): Temporary Implementation: Delete this function definition when all the filter +// nodes conform to new API. +void ScanGroundFilterComponent::filter( + const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, + PointCloud2 & output) +{ + (void)input; + (void)indices; + (void)output; +} + rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( const std::vector & p) { + if (get_param(p, "grid_size_m", grid_size_m_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_m to: %f.", grid_size_m_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } + if (get_param(p, "grid_mode_switch_radius", grid_mode_switch_radius_)) { + grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; + grid_mode_switch_angle_rad_ = std::atan2(grid_mode_switch_radius_, virtual_lidar_z_); + grid_size_rad_ = + normalizeRadian(std::atan2(grid_mode_switch_radius_ + grid_size_m_, virtual_lidar_z_)) - + normalizeRadian(std::atan2(grid_mode_switch_radius_, virtual_lidar_z_)); + tan_grid_size_rad_ = std::tan(grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_mode_switch_radius to: %f.", grid_mode_switch_radius_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_grid_id to: %f.", grid_mode_switch_grid_id_); + RCLCPP_DEBUG( + get_logger(), "Setting grid_mode_switch_angle_rad to: %f.", grid_mode_switch_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting grid_size_rad to: %f.", grid_size_rad_); + RCLCPP_DEBUG(get_logger(), "Setting tan_grid_size_rad to: %f.", tan_grid_size_rad_); + } double global_slope_max_angle_deg{get_parameter("global_slope_max_angle_deg").as_double()}; if (get_param(p, "global_slope_max_angle_deg", global_slope_max_angle_deg)) { global_slope_max_angle_rad_ = deg2rad(global_slope_max_angle_deg); + global_slope_max_ratio_ = std::tan(global_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting global_slope_max_angle_rad to: %f.", global_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting global_slope_max_ratio to: %f.", global_slope_max_ratio_); } double local_slope_max_angle_deg{get_parameter("local_slope_max_angle_deg").as_double()}; if (get_param(p, "local_slope_max_angle_deg", local_slope_max_angle_deg)) { local_slope_max_angle_rad_ = deg2rad(local_slope_max_angle_deg); + local_slope_max_ratio_ = std::tan(local_slope_max_angle_rad_); RCLCPP_DEBUG( get_logger(), "Setting local_slope_max_angle_rad to: %f.", local_slope_max_angle_rad_); + RCLCPP_DEBUG(get_logger(), "Setting local_slope_max_ratio to: %f.", local_slope_max_ratio_); } double radial_divider_angle_deg{get_parameter("radial_divider_angle_deg").as_double()}; if (get_param(p, "radial_divider_angle_deg", radial_divider_angle_deg)) { @@ -602,9 +696,14 @@ rcl_interfaces::msg::SetParametersResult ScanGroundFilterComponent::onParameter( RCLCPP_DEBUG(get_logger(), "Setting radial_dividers_num to: %zu.", radial_dividers_num_); } if (get_param(p, "split_points_distance_tolerance", split_points_distance_tolerance_)) { + split_points_distance_tolerance_square_ = + split_points_distance_tolerance_ * split_points_distance_tolerance_; RCLCPP_DEBUG( get_logger(), "Setting split_points_distance_tolerance to: %f.", split_points_distance_tolerance_); + RCLCPP_DEBUG( + get_logger(), "Setting split_points_distance_tolerance_square to: %f.", + split_points_distance_tolerance_square_); } if (get_param(p, "split_height_distance", split_height_distance_)) { RCLCPP_DEBUG(get_logger(), "Setting split_height_distance to: %f.", split_height_distance_); diff --git a/perception/ground_segmentation/test/test_ray_ground_filter.cpp b/perception/ground_segmentation/test/test_ray_ground_filter.cpp index af8bb29ab12b7..0c1db47a5ae09 100644 --- a/perception/ground_segmentation/test/test_ray_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_ray_ground_filter.cpp @@ -29,7 +29,7 @@ #include #include #endif - +#include class RayGroundFilterComponentTestSuite : public ::testing::Test { protected: @@ -63,8 +63,25 @@ class RayGroundFilterComponentTest : public ground_segmentation::RayGroundFilter TEST_F(RayGroundFilterComponentTestSuite, TestCase1) { - // read pcd to pointcloud const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); + const auto config_path = share_dir + "/config/ray_ground_filter.param.yaml"; + // std::cout << "config_path:" << config_path << std::endl; + YAML::Node config = YAML::LoadFile(config_path); + auto params = config["/**"]["ros__parameters"]; + + double min_x_ = params["min_x"].as(); + double max_x_ = params["max_x"].as(); + double min_y_ = params["min_y"].as(); + double max_y_ = params["max_y"].as(); + bool use_vehicle_footprint_ = params["use_vehicle_footprint"].as(); + double general_max_slope_ = params["general_max_slope"].as(); + double local_max_slope_ = params["local_max_slope"].as(); + double initial_max_slope_ = params["initial_max_slope"].as(); + double radial_divider_angle_ = params["radial_divider_angle"].as(); + double min_height_threshold_ = params["min_height_threshold"].as(); + double concentric_divider_distance_ = params["concentric_divider_distance"].as(); + double reclass_distance_threshold_ = params["reclass_distance_threshold"].as(); + const auto pcd_path = share_dir + "/data/test.pcd"; pcl::PointCloud cloud; pcl::io::loadPCDFile(pcd_path, cloud); @@ -94,10 +111,23 @@ TEST_F(RayGroundFilterComponentTestSuite, TestCase1) rclcpp::NodeOptions node_options; std::vector parameters; + parameters.emplace_back(rclcpp::Parameter("base_frame", "base_link")); - parameters.emplace_back(rclcpp::Parameter("general_max_slope", 2.0)); - parameters.emplace_back(rclcpp::Parameter("local_max_slope", 3.0)); - parameters.emplace_back(rclcpp::Parameter("initial_max_slope", 1.0)); + parameters.emplace_back(rclcpp::Parameter("general_max_slope", general_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("local_max_slope", local_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("initial_max_slope", initial_max_slope_)); + parameters.emplace_back(rclcpp::Parameter("radial_divider_angle", radial_divider_angle_)); + parameters.emplace_back(rclcpp::Parameter("min_height_threshold", min_height_threshold_)); + parameters.emplace_back( + rclcpp::Parameter("concentric_divider_distance", concentric_divider_distance_)); + parameters.emplace_back( + rclcpp::Parameter("reclass_distance_threshold", reclass_distance_threshold_)); + parameters.emplace_back(rclcpp::Parameter("min_x", min_x_)); + parameters.emplace_back(rclcpp::Parameter("max_x", max_x_)); + parameters.emplace_back(rclcpp::Parameter("min_y", min_y_)); + parameters.emplace_back(rclcpp::Parameter("max_y", max_y_)); + parameters.emplace_back(rclcpp::Parameter("use_vehicle_footprint", use_vehicle_footprint_)); + node_options.parameter_overrides(parameters); auto ray_ground_filter_test = std::make_shared(node_options); ray_ground_filter_test->input_pointcloud_pub_->publish(*input_msg_ptr); diff --git a/perception/ground_segmentation/test/test_scan_ground_filter.cpp b/perception/ground_segmentation/test/test_scan_ground_filter.cpp index acc81f8817cd8..e48bd36d8c54e 100644 --- a/perception/ground_segmentation/test/test_scan_ground_filter.cpp +++ b/perception/ground_segmentation/test/test_scan_ground_filter.cpp @@ -38,6 +38,8 @@ class ScanGroundFilterTest : public ::testing::Test { rclcpp::init(0, nullptr); + parse_yaml(); + dummy_node_ = std::make_shared("ScanGroundFilterTest"); input_pointcloud_pub_ = rclcpp::create_publisher( dummy_node_, "/test_scan_ground_filter/input_cloud", 1); @@ -58,6 +60,31 @@ class ScanGroundFilterTest : public ::testing::Test parameters.emplace_back(rclcpp::Parameter("right_overhang", 0.1)); parameters.emplace_back(rclcpp::Parameter("vehicle_height", 2.5)); parameters.emplace_back(rclcpp::Parameter("max_steer_angle", 0.7)); + + parameters.emplace_back( + rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); + parameters.emplace_back( + rclcpp::Parameter("use_virtual_ground_point", use_virtual_ground_point_)); + parameters.emplace_back(rclcpp::Parameter("split_height_distance", split_height_distance_)); + parameters.emplace_back( + rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); + parameters.emplace_back(rclcpp::Parameter("grid_size_m", grid_size_m_)); + parameters.emplace_back(rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); + parameters.emplace_back(rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); + parameters.emplace_back(rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); + parameters.emplace_back(rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); + parameters.emplace_back(rclcpp::Parameter("low_priority_region_x", low_priority_region_x_)); + parameters.emplace_back(rclcpp::Parameter("center_pcl_shift", center_pcl_shift_)); + parameters.emplace_back( + rclcpp::Parameter("radial_divider_angle_deg", radial_divider_angle_deg_)); + parameters.emplace_back( + rclcpp::Parameter("use_recheck_ground_cluster", use_recheck_ground_cluster_)); + parameters.emplace_back(rclcpp::Parameter("use_lowest_point", use_lowest_point_)); + options.parameter_overrides(parameters); scan_ground_filter_ = std::make_shared(options); @@ -88,8 +115,6 @@ class ScanGroundFilterTest : public ::testing::Test t.transform.rotation.w = q.w(); tf2::doTransform(*origin_input_msg_ptr, *input_msg_ptr_, t); - - parse_yaml(); } ScanGroundFilterTest() {} @@ -107,16 +132,17 @@ class ScanGroundFilterTest : public ::testing::Test // wrapper function to test private function filter void filter(sensor_msgs::msg::PointCloud2 & out_cloud) { - scan_ground_filter_->filter(input_msg_ptr_, nullptr, out_cloud); + pointcloud_preprocessor::TransformInfo transform_info; + scan_ground_filter_->faster_filter(input_msg_ptr_, nullptr, out_cloud, transform_info); } void parse_yaml() { const auto share_dir = ament_index_cpp::get_package_share_directory("ground_segmentation"); - const auto config_path = share_dir + "/config/ground_segmentation.param.yaml"; + const auto config_path = share_dir + "/config/scan_ground_filter.param.yaml"; // std::cout << "config_path:" << config_path << std::endl; YAML::Node config = YAML::LoadFile(config_path); - auto params = config["/**"]["ros__parameters"]["common_ground_filter"]["parameters"]; + auto params = config["/**"]["ros__parameters"]; global_slope_max_angle_deg_ = params["global_slope_max_angle_deg"].as(); local_slope_max_angle_deg_ = params["local_slope_max_angle_deg"].as(); split_points_distance_tolerance_ = params["split_points_distance_tolerance"].as(); @@ -127,6 +153,12 @@ class ScanGroundFilterTest : public ::testing::Test gnd_grid_buffer_size_ = params["gnd_grid_buffer_size"].as(); detection_range_z_max_ = params["detection_range_z_max"].as(); elevation_grid_mode_ = params["elevation_grid_mode"].as(); + low_priority_region_x_ = params["low_priority_region_x"].as(); + use_virtual_ground_point_ = params["use_virtual_ground_point"].as(); + center_pcl_shift_ = params["center_pcl_shift"].as(); + radial_divider_angle_deg_ = params["radial_divider_angle_deg"].as(); + use_recheck_ground_cluster_ = params["use_recheck_ground_cluster"].as(); + use_lowest_point_ = params["use_lowest_point"].as(); } float global_slope_max_angle_deg_ = 0.0; @@ -139,6 +171,12 @@ class ScanGroundFilterTest : public ::testing::Test uint16_t gnd_grid_buffer_size_ = 0; float detection_range_z_max_ = 0.0; bool elevation_grid_mode_ = false; + float low_priority_region_x_ = 0.0; + bool use_virtual_ground_point_; + float center_pcl_shift_; + float radial_divider_angle_deg_; + bool use_recheck_ground_cluster_; + bool use_lowest_point_; }; TEST_F(ScanGroundFilterTest, TestCase1) @@ -146,27 +184,6 @@ TEST_F(ScanGroundFilterTest, TestCase1) input_pointcloud_pub_->publish(*input_msg_ptr_); sensor_msgs::msg::PointCloud2 out_cloud; - // set filter parameter - scan_ground_filter_->set_parameter( - rclcpp::Parameter("global_slope_max_angle_deg", global_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("local_slope_max_angle_deg", local_slope_max_angle_deg_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_points_distance_tolerance", split_points_distance_tolerance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("split_height_distance", split_height_distance_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("non_ground_height_threshold", non_ground_height_threshold_)); - scan_ground_filter_->set_parameter(rclcpp::Parameter("grid_size_m", grid_size_m_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("grid_mode_switch_radius", grid_mode_switch_radius_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("gnd_grid_buffer_size", gnd_grid_buffer_size_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("detection_range_z_max", detection_range_z_max_)); - scan_ground_filter_->set_parameter( - rclcpp::Parameter("elevation_grid_mode", elevation_grid_mode_)); - filter(out_cloud); output_pointcloud_pub_->publish(out_cloud); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index f89c83e9c2d5a..85535df4babfe 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -415,17 +415,21 @@ bool withinRoadLanelet( } boost::optional isReachableCrosswalkEdgePoints( - const TrackedObject & object, const CrosswalkEdgePoints & edge_points, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const double time_horizon, - const double min_object_vel) + const TrackedObject & object, const lanelet::ConstLanelet & target_crosswalk, + const CrosswalkEdgePoints & edge_points, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const double time_horizon, const double min_object_vel) { using Point = boost::geometry::model::d2::point_xy; - using Line = boost::geometry::model::linestring; const auto & obj_pos = object.kinematics.pose_with_covariance.pose.position; const auto & obj_vel = object.kinematics.twist_with_covariance.twist.linear; const auto yaw = tier4_autoware_utils::getRPY(object.kinematics.pose_with_covariance.pose).z; + lanelet::BasicPoint2d obj_pos_as_lanelet(obj_pos.x, obj_pos.y); + if (boost::geometry::within(obj_pos_as_lanelet, target_crosswalk.polygon2d().basicPolygon())) { + return {}; + } + const auto & p1 = edge_points.front_center_point; const auto & p2 = edge_points.back_center_point; @@ -442,62 +446,66 @@ boost::optional isReachableCrosswalkEdgePoints( const auto estimated_velocity = std::hypot(obj_vel.x, obj_vel.y); const auto is_stop_object = estimated_velocity < stop_velocity_th; const auto velocity = std::max(min_object_vel, estimated_velocity); - - lanelet::BasicPoint2d search_point(obj_pos.x, obj_pos.y); - // nearest lanelet const auto surrounding_lanelets = lanelet::geometry::findNearest( - lanelet_map_ptr->laneletLayer, search_point, time_horizon * velocity); + lanelet_map_ptr->laneletLayer, obj_pos_as_lanelet, time_horizon * velocity); - bool first_intersect_load = false; - bool second_intersect_load = false; - std::vector intersects_first; - std::vector intersects_second; - for (const auto & lanelet : surrounding_lanelets) { - if (withinLanelet(object, lanelet.second)) { - return {}; - } + const auto isAcrossAnyRoad = [&surrounding_lanelets](const Point & p_src, const Point & p_dst) { + const auto withinAnyCrosswalk = [&surrounding_lanelets](const Point & p) { + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if ( + (attr.value() == lanelet::AttributeValueString::Crosswalk || + attr.value() == lanelet::AttributeValueString::Walkway) && + boost::geometry::within(p, lanelet.second.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; - lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); - if (attr.value() != "road") { - continue; - } + const auto isExist = [](const Point & p, const std::vector & points) { + for (const auto & existingPoint : points) { + if (boost::geometry::distance(p, existingPoint) < 1e-1) { + return true; + } + } + return false; + }; - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}}; - std::vector tmp_intersects; - boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); - for (const auto & p : tmp_intersects) { - intersects_first.push_back(p); + std::vector points_of_intersect; + const boost::geometry::model::linestring line{p_src, p_dst}; + for (const auto & lanelet : surrounding_lanelets) { + const lanelet::Attribute attr = lanelet.second.attribute(lanelet::AttributeName::Subtype); + if (attr.value() != lanelet::AttributeValueString::Road) { + continue; } - } - { - const Line object_to_entry_point{ - {obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}}; std::vector tmp_intersects; boost::geometry::intersection( - object_to_entry_point, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); + line, lanelet.second.polygon2d().basicPolygon(), tmp_intersects); for (const auto & p : tmp_intersects) { - intersects_second.push_back(p); + if (isExist(p, points_of_intersect) || withinAnyCrosswalk(p)) { + continue; + } + points_of_intersect.push_back(p); + if (points_of_intersect.size() >= 2) { + return true; + } } } - } - - if (1 < intersects_first.size()) { - first_intersect_load = true; - } + return false; + }; - if (1 < intersects_second.size()) { - second_intersect_load = true; - } + const bool first_intersects_road = isAcrossAnyRoad( + {obj_pos.x, obj_pos.y}, {ret.front_center_point.x(), ret.front_center_point.y()}); + const bool second_intersects_road = + isAcrossAnyRoad({obj_pos.x, obj_pos.y}, {ret.back_center_point.x(), ret.back_center_point.y()}); - if (first_intersect_load && second_intersect_load) { + if (first_intersects_road && second_intersects_road) { return {}; } - if (first_intersect_load && !second_intersect_load) { + if (first_intersects_road && !second_intersects_road) { ret.swap(); } @@ -606,7 +614,7 @@ ObjectClassification::_label_type changeLabelForPrediction( // constraints const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float high_speed_threshold = - tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + tier4_autoware_utils::kmph2mps(9.0); // For lv4 special tuning. High speed bicycle 9 km/h // calc abs speed from x and y velocity const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, @@ -624,17 +632,17 @@ ObjectClassification::_label_type changeLabelForPrediction( case ObjectClassification::PEDESTRIAN: { const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); const float max_velocity_for_human_mps = - tier4_autoware_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + tier4_autoware_utils::kmph2mps(9.0); // Max human being motion speed is 25km/h const double abs_speed = std::hypot( object.kinematics.twist_with_covariance.twist.linear.x, object.kinematics.twist_with_covariance.twist.linear.y); const bool high_speed_object = abs_speed > max_velocity_for_human_mps; // fast, human-like object: like segway - if (within_road_lanelet && high_speed_object) return label; // currently do nothing - // return ObjectClassification::MOTORCYCLE; - if (high_speed_object) return label; // currently do nothing - // fast human outside road lanelet will move like unknown object - // return ObjectClassification::UNKNOWN; + if (within_road_lanelet && high_speed_object) // return label; // currently do nothing + return ObjectClassification::MOTORCYCLE; + if (high_speed_object) // return label; // currently do nothing + // fast human outside road lanelet will move like unknown object + return ObjectClassification::UNKNOWN; return label; } @@ -1135,48 +1143,45 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( predicted_object.kinematics.predicted_paths.push_back(predicted_path); } } + } + // try to find the edge points for all crosswalks and generate path to the crosswalk edge + for (const auto & crosswalk : crosswalks_) { + const auto edge_points = getCrosswalkEdgePoints(crosswalk); + + const auto reachable_first = hasPotentialToReach( + object, edge_points.front_center_point, edge_points.front_right_point, + edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + const auto reachable_second = hasPotentialToReach( + object, edge_points.back_center_point, edge_points.back_right_point, + edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, + max_crosswalk_user_delta_yaw_threshold_for_lanelet_); + + if (!reachable_first && !reachable_second) { + continue; + } - // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge - // points for all crosswalks and generate path to the crosswalk edge - } else { - for (const auto & crosswalk : crosswalks_) { - const auto edge_points = getCrosswalkEdgePoints(crosswalk); - - const auto reachable_first = hasPotentialToReach( - object, edge_points.front_center_point, edge_points.front_right_point, - edge_points.front_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - const auto reachable_second = hasPotentialToReach( - object, edge_points.back_center_point, edge_points.back_right_point, - edge_points.back_left_point, prediction_time_horizon_, min_crosswalk_user_velocity_, - max_crosswalk_user_delta_yaw_threshold_for_lanelet_); - - if (!reachable_first && !reachable_second) { - continue; - } - - const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( - object, edge_points, lanelet_map_ptr_, prediction_time_horizon_, - min_crosswalk_user_velocity_); - - if (!reachable_crosswalk) { - continue; - } + const auto reachable_crosswalk = isReachableCrosswalkEdgePoints( + object, crosswalk, edge_points, lanelet_map_ptr_, prediction_time_horizon_, + min_crosswalk_user_velocity_); - PredictedPath predicted_path = - path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); - predicted_path.confidence = 1.0; + if (!reachable_crosswalk) { + continue; + } - if (predicted_path.path.empty()) { - continue; - } - // If the predicted path to the crosswalk is crossing the fence, don't use it - if (doesPathCrossAnyFence(predicted_path)) { - continue; - } + PredictedPath predicted_path = + path_generator_->generatePathForCrosswalkUser(object, reachable_crosswalk.get()); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); + if (predicted_path.path.empty()) { + continue; } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; + } + + predicted_object.kinematics.predicted_paths.push_back(predicted_path); } const auto n_path = predicted_object.kinematics.predicted_paths.size(); diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp index ad268283d5890..ba50933eddaec 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -32,16 +32,18 @@ class BicycleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; @@ -51,7 +53,7 @@ class BicycleTracker : public Tracker float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 8a9b6adfc9cd5..e19b6382ad952 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -32,26 +32,28 @@ class BigVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 79e9a20a10421..05fa0a5ef01ba 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -33,28 +33,30 @@ class NormalVehicleTracker : public Tracker private: KalmanFilter ekf_; rclcpp::Time last_update_time_; - enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 }; + enum IDX { X = 0, Y = 1, YAW = 2, VEL = 3, SLIP = 4 }; struct EkfParams { char dim_x = 5; - float q_cov_x; - float q_cov_y; - float q_cov_yaw; - float q_cov_slip; - float q_cov_vx; - float p0_cov_vx; + float q_stddev_acc_long; + float q_stddev_acc_lat; + float q_stddev_yaw_rate_min; + float q_stddev_yaw_rate_max; + float q_cov_slip_rate_min; + float q_cov_slip_rate_max; + float q_max_slip_angle; + float p0_cov_vel; float p0_cov_slip; float r_cov_x; float r_cov_y; float r_cov_yaw; - float r_cov_vx; + float r_cov_vel; float p0_cov_x; float p0_cov_y; float p0_cov_yaw; } ekf_params_; - double max_vx_; + double max_vel_; double max_slip_; double lf_; double lr_; @@ -88,6 +90,7 @@ class NormalVehicleTracker : public Tracker const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const override; void setNearestCornerOrSurfaceIndex(const geometry_msgs::msg::Transform & self_transform); + double getMeasurementYaw(const autoware_auto_perception_msgs::msg::DetectedObject & object); virtual ~NormalVehicleTracker() {} }; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp index 95c23d8e42019..d2850793fa4bb 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp @@ -42,6 +42,8 @@ class Tracker { classification_ = classification; } + void updateClassification( + const std::vector & classification); private: unique_identifier_msgs::msg::UUID uuid_; diff --git a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp index d2b6ee5de475e..123ce50f62b0f 100644 --- a/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp +++ b/perception/multi_object_tracker/include/multi_object_tracker/utils/utils.hpp @@ -313,15 +313,12 @@ inline void convertConvexHullToBoundingBox( // look for bounding box boundary for (size_t i = 0; i < input_object.shape.footprint.points.size(); ++i) { - Eigen::Vector2d vertex{ - input_object.shape.footprint.points.at(i).x, input_object.shape.footprint.points.at(i).y}; - - const Eigen::Vector2d local_vertex = R_inv * (vertex - center); - max_x = std::max(max_x, local_vertex.x()); - max_y = std::max(max_y, local_vertex.y()); - min_x = std::min(min_x, local_vertex.x()); - min_y = std::min(min_y, local_vertex.y()); - + const double foot_x = input_object.shape.footprint.points.at(i).x; + const double foot_y = input_object.shape.footprint.points.at(i).y; + max_x = std::max(max_x, foot_x); + max_y = std::max(max_y, foot_y); + min_x = std::min(min_x, foot_x); + min_y = std::min(min_y, foot_y); max_z = std::max(max_z, static_cast(input_object.shape.footprint.points.at(i).z)); } diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index f74967943be32..f1f53c7398c0a 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -11,21 +11,21 @@ CTRV model is a model that assumes constant turn rate and velocity magnitude. - state transition equation $$ -\begin{align*} -x_{k+1} &= x_k + v_{x_k} \cos(\psi_k) \cdot dt \\ -y_{k+1} &= y_k + v_{x_k} \sin(\psi_k) \cdot dt \\ -\psi_{k+1} &= \psi_k + \dot{\psi}_k \cdot dt \\ -v_{x_{k+1}} &= v_{x_k} \\ -\dot{\psi}_{k+1} &= \dot{\psi}_k \\ -\end{align*} +\begin{aligned} +x_{k+1} & = x_{k} + v_{k} \cos(\psi_k) \cdot {d t} \\ +y_{k+1} & = y_{k} + v_{k} \sin(\psi_k) \cdot {d t} \\ +\psi_{k+1} & = \psi_k + \dot\psi_{k} \cdot {d t} \\ +v_{k+1} & = v_{k} \\ +\dot\psi_{k+1} & = \dot\psi_{k} \\ +\end{aligned} $$ - jacobian $$ A = \begin{bmatrix} -1 & 0 & -v_x \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ -0 & 1 & v_x \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ +1 & 0 & -v \sin(\psi) \cdot dt & \cos(\psi) \cdot dt & 0 \\ +0 & 1 & v \cos(\psi) \cdot dt & \sin(\psi) \cdot dt & 0 \\ 0 & 0 & 1 & 0 & dt \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ @@ -40,17 +40,20 @@ The merit of using this model is that it can prevent unintended yaw rotation whe ![kinematic_bicycle_model](image/kinematic_bicycle_model.png) - **state variable** - - pose( $x,y$ ), velocity( $v$ ), yaw( $\psi$ ), and slip angle ( $\beta$ ) - - $[x_{k} ,y_{k} , v_{k} , \psi_{k} , \beta_{k} ]^\mathrm{T}$ + - pose( $x,y$ ), yaw( $\psi$ ), velocity( $v$ ), and slip angle ( $\beta$ ) + - $[x_{k}, y_{k}, \psi_{k}, v_{k}, \beta_{k} ]^\mathrm{T}$ - **Prediction Equation** - $dt$: sampling time + - $w_{k} = \dot\psi_{k} = \frac{ v_{k} \sin \left( \beta_{k} \right) }{ l_r }$ : angular velocity $$ \begin{aligned} -x_{k+1} & =x_{k}+v_{k} \cos \left(\psi_{k}+\beta_{k}\right) d t \\ -y_{k+1} & =y_{k}+v_{k} \sin \left(\psi_{k}+\beta_{k}\right) d t \\ -v_{k+1} & =v_{k} \\ -\psi_{k+1} & =\psi_{k}+\frac{v_{k}}{l_{r}} \sin \beta_{k} d t \\ +x_{k+1} & = x_{k} + v_{k} \cos \left( \psi_{k}+\beta_{k} \right) {d t} + -\frac{1}{2} \left\lbrace w_k v_k \sin \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +y_{k+1} & = y_{k} + v_{k} \sin \left( \psi_{k}+\beta_{k} \right) {d t} + +\frac{1}{2} \left\lbrace w_k v_k \cos \left(\psi_{k}+\beta_{k} \right) \right\rbrace {d t}^2\\ +\psi_{k+1} & =\psi_{k} + w_k {d t} \\ +v_{k+1} & = v_{k} \\ \beta_{k+1} & =\beta_{k} \end{aligned} $$ @@ -59,9 +62,15 @@ $$ $$ \frac{\partial f}{\partial \mathrm x}=\left[\begin{array}{ccccc} -1 & 0 & -v \sin (\psi+\beta) d t & v \cos (\psi+\beta) & -v \sin (\psi+\beta) d t \\ -0 & 1 & v \cos (\psi+\beta) d t & v \sin (\psi+\beta) & v \cos (\psi+\beta) d t \\ -0 & 0 & 1 & \frac{1}{l_r} \sin \beta d t & \frac{v}{l_r} \cos \beta d t \\ +1 & 0 + & v \cos (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \sin (\psi+\beta) {d t} - \left\lbrace w \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & -v \sin (\psi+\beta) {d t} - \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\sin(\psi+\beta)+\sin(\beta)\cos(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 1 + & v \sin (\psi+\beta) {d t} - \frac{1}{2} \left\lbrace w v \sin \left( \psi+\beta \right) \right\rbrace {d t}^2 + & \cos (\psi+\beta) {d t} + \left\lbrace w \cos \left( \psi+\beta \right) \right\rbrace {d t}^2 + & v \cos (\psi+\beta) {d t} + \frac{v^2}{2l_r} \left\lbrace \cos(\beta)\cos(\psi+\beta)-\sin(\beta)\sin(\psi+\beta) \right\rbrace {d t}^2 \\ +0 & 0 & 1 & \frac{1}{l_r} \sin \beta {d t} & \frac{v}{l_r} \cos \beta d t \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{array}\right] diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index e3172dfd22349..119919b5f076c 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -13,6 +13,7 @@ eigen3_cmake_module autoware_auto_perception_msgs + diagnostic_updater eigen kalman_filter mussp @@ -24,7 +25,6 @@ tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs - ament_lint_auto autoware_lint_common diff --git a/perception/multi_object_tracker/src/data_association/data_association.cpp b/perception/multi_object_tracker/src/data_association/data_association.cpp index 5e7cadc423216..f367e19c11f2a 100644 --- a/perception/multi_object_tracker/src/data_association/data_association.cpp +++ b/perception/multi_object_tracker/src/data_association/data_association.cpp @@ -201,7 +201,7 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( measurement_object.kinematics.pose_with_covariance.pose.position, tracked_object.kinematics.pose_with_covariance.pose.position, getXYCovariance(tracked_object.kinematics.pose_with_covariance)); - if (2.448 /*95%*/ <= mahalanobis_dist) passed_gate = false; + if (3.035 /*99%*/ <= mahalanobis_dist) passed_gate = false; } // 2d iou gate if (passed_gate) { diff --git a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp index 6571e70b8c123..8a3a5629ec277 100644 --- a/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp @@ -52,51 +52,57 @@ BicycleTracker::BicycleTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.6; // [m/s] - float q_stddev_y = 0.6; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)] - float r_stddev_x = 0.6; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 0.8; // [m/s] - float p0_stddev_y = 0.5; // [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // initial state covariance + float p0_stddev_x = 0.8; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - - // initialize X matrix + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(5.0); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(1); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); const double sin_yaw = std::sin(X(IDX::YAW)); @@ -110,7 +116,7 @@ BicycleTracker::BicycleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -120,10 +126,10 @@ BicycleTracker::BicycleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -137,15 +143,28 @@ BicycleTracker::BicycleTracker( ekf_.init(X, P); // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m } bool BicycleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -154,69 +173,116 @@ bool BicycleTracker::predict(const rclcpp::Time & time) bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* Motion model: static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -225,12 +291,6 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const return true; } -/** - * @brief measurement update with ekf on receiving detected_object - * - * @param object : Detected object - * @return bool : measurement is updatable - */ bool BicycleTracker::measureWithPose( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -267,19 +327,16 @@ bool BicycleTracker::measureWithPose( const int dim_y = use_orientation_information ? 3 : 2; // pos x, pos y, (pos yaw) depending on Pose output - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Y(IDX::X, 0) = object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y - /* Set measurement noise covariance */ + // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - if (!object.kinematics.has_position_covariance) { R(0, 0) = ekf_params_.r_cov_x; // x - x R(0, 1) = 0.0; // x - y @@ -310,20 +367,20 @@ bool BicycleTracker::measureWithPose( } } - // update with ekf + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -341,14 +398,26 @@ bool BicycleTracker::measureWithPose( bool BicycleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type + autoware_auto_perception_msgs::msg::DetectedObject bbox_object; if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { - return false; + utils::convertConvexHullToBoundingBox(object, bbox_object); + } else { + bbox_object = object; } - constexpr float gain = 0.9; - bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; - bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; - bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + constexpr float gain = 0.9; + bounding_box_.length = + gain * bounding_box_.length + (1.0 - gain) * bbox_object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * bbox_object.shape.dimensions.y; + bounding_box_.height = + gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; + last_input_bounding_box_ = { + bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% front from the center, minimum of 0.3m + lr_ = std::max(bounding_box_.length * 0.3, 0.3); // 30% rear from the center, minimum of 0.3m return true; } @@ -382,7 +451,7 @@ bool BicycleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -393,6 +462,7 @@ bool BicycleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -429,23 +499,44 @@ bool BicycleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp index 97d6d48c35d1b..80c0df7e5ffb1 100644 --- a/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/big_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,53 +53,59 @@ BigVehicleTracker::BigVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.5; // [m/s] - float q_stddev_y = 1.5; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // [rad/(s*s)] - float r_stddev_x = 1.5; // [m] - float r_stddev_y = 0.5; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float r_stddev_vx = 1.0; // [m/s] - float p0_stddev_x = 1.5; // [m] - float p0_stddev_y = 0.5; // [m] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad] + float r_stddev_vel = 1.0; // [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.5; // [m] + float p0_stddev_y = 0.5; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; - // X(IDX::YAW) = object.kinematics.twist_with_covariance.twist.angular.z; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -112,7 +120,7 @@ BigVehicleTracker::BigVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -122,10 +130,10 @@ BigVehicleTracker::BigVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -142,9 +150,7 @@ BigVehicleTracker::BigVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ BigVehicleTracker::BigVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m } bool BigVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool BigVehicleTracker::predict(const rclcpp::Time & time) bool BigVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* Motion model: static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -249,14 +315,14 @@ bool BigVehicleTracker::measureWithPose( float r_cov_y; const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == Label::CAR) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] - r_cov_x = std::pow(r_stddev_x, 2.0); - r_cov_y = std::pow(r_stddev_y, 2.0); - } else if (utils::isLargeVehicleLabel(label)) { + if (utils::isLargeVehicleLabel(label)) { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; + } else if (label == Label::CAR) { + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] + r_cov_x = std::pow(r_stddev_x, 2.0); + r_cov_y = std::pow(r_stddev_y, 2.0); } else { r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; @@ -267,16 +333,16 @@ bool BigVehicleTracker::measureWithPose( if (object.kinematics.has_twist) { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf_.getX(X_t); - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose measurement + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -296,11 +362,9 @@ bool BigVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; @@ -308,7 +372,8 @@ bool BigVehicleTracker::measureWithPose( C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(measurement_yaw); const double sin_yaw = std::sin(measurement_yaw); @@ -330,31 +395,32 @@ bool BigVehicleTracker::measureWithPose( R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - /* ekf tracks constant tracking point */ + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, slip + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -372,9 +438,8 @@ bool BigVehicleTracker::measureWithPose( bool BigVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -389,6 +454,11 @@ bool BigVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 1.5); // 30% front from the center, minimum of 1.5m + lr_ = std::max(bounding_box_.length * 0.25, 1.5); // 25% rear from the center, minimum of 1.5m + return true; } @@ -480,23 +550,44 @@ bool BigVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; diff --git a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp index 51adca7e69b56..4f0fb4d7871f2 100644 --- a/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/multiple_vehicle_tracker.cpp @@ -43,7 +43,7 @@ bool MultipleVehicleTracker::measure( big_vehicle_tracker_.measure(object, time, self_transform); normal_vehicle_tracker_.measure(object, time, self_transform); if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) - setClassification(object.classification); + updateClassification(object.classification); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp index aa3f7b1c30d01..7714c381894f0 100644 --- a/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp @@ -16,6 +16,14 @@ // Author: v1.0 Yukihiro Saito // +#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" + +#include "multi_object_tracker/utils/utils.hpp" + +#include +#include +#include + #include #include #include @@ -26,17 +34,11 @@ #else #include #endif - -#define EIGEN_MPL2_ONLY -#include "multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp" -#include "multi_object_tracker/utils/utils.hpp" #include "object_recognition_utils/object_recognition_utils.hpp" +#define EIGEN_MPL2_ONLY #include #include -#include -#include -#include using Label = autoware_auto_perception_msgs::msg::ObjectClassification; @@ -51,52 +53,59 @@ NormalVehicleTracker::NormalVehicleTracker( { object_ = object; - // initialize params - float q_stddev_x = 1.0; // object coordinate [m/s] - float q_stddev_y = 1.0; // object coordinate [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // map coordinate[rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // object coordinate [m/(s*s)] - float q_stddev_slip = tier4_autoware_utils::deg2rad(5); // object coordinate [rad/(s*s)] - float r_stddev_x = 1.0; // object coordinate [m] - float r_stddev_y = 0.3; // object coordinate [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float r_stddev_vx = 1.0; // object coordinate [m/s] - float p0_stddev_x = 1.0; // object coordinate [m/s] - float p0_stddev_y = 0.3; // object coordinate [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(30); // map coordinate [rad] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // object coordinate [m/s] - float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // object coordinate [rad/s] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0); + // Initialize parameters + // measurement noise covariance: detector uncertainty + ego vehicle motion uncertainty + float r_stddev_x = 0.5; // in vehicle coordinate [m] + float r_stddev_y = 0.4; // in vehicle coordinate [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(20); // in map coordinate [rad] + float r_stddev_vel = 1.0; // in object coordinate [m/s] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); - ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.r_cov_vel = std::pow(r_stddev_vel, 2.0); + // initial state covariance + float p0_stddev_x = 1.0; // in object coordinate [m] + float p0_stddev_y = 0.3; // in object coordinate [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(25); // in map coordinate [rad] + float p0_stddev_vel = tier4_autoware_utils::kmph2mps(1000); // in object coordinate [m/s] + float p0_stddev_slip = tier4_autoware_utils::deg2rad(5); // in object coordinate [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); - ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_vel = std::pow(p0_stddev_vel, 2.0); ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0); - max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s] - max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s] + // process noise covariance + ekf_params_.q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration + ekf_params_.q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration + ekf_params_.q_stddev_yaw_rate_min = + tier4_autoware_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate + ekf_params_.q_stddev_yaw_rate_max = + tier4_autoware_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate + float q_stddev_slip_rate_min = + tier4_autoware_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate + float q_stddev_slip_rate_max = + tier4_autoware_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate + ekf_params_.q_cov_slip_rate_min = std::pow(q_stddev_slip_rate_min, 2.0); + ekf_params_.q_cov_slip_rate_max = std::pow(q_stddev_slip_rate_max, 2.0); + ekf_params_.q_max_slip_angle = tier4_autoware_utils::deg2rad(30); // [rad] max slip angle + // limitations + max_vel_ = tier4_autoware_utils::kmph2mps(100); // [m/s] + max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad] velocity_deviation_threshold_ = tier4_autoware_utils::kmph2mps(10); // [m/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + X(IDX::SLIP) = 0.0; if (object.kinematics.has_twist) { - X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VEL) = object.kinematics.twist_with_covariance.twist.linear.x; } else { - X(IDX::VX) = 0.0; + X(IDX::VEL) = 0.0; } - X(IDX::SLIP) = 0.0; - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -111,7 +120,7 @@ NormalVehicleTracker::NormalVehicleTracker( ekf_params_.p0_cov_x * sin_yaw * sin_yaw + ekf_params_.p0_cov_y * cos_yaw * cos_yaw; P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y); P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } else { P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; @@ -121,10 +130,10 @@ NormalVehicleTracker::NormalVehicleTracker( P(IDX::YAW, IDX::YAW) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; if (object.kinematics.has_twist_covariance) { - P(IDX::VX, IDX::VX) = + P(IDX::VEL, IDX::VEL) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } else { - P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + P(IDX::VEL, IDX::VEL) = ekf_params_.p0_cov_vel; } P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip; } @@ -132,8 +141,7 @@ NormalVehicleTracker::NormalVehicleTracker( if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; - last_input_bounding_box_ = { - object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } else { // past default value // bounding_box_ = {4.0, 1.7, 2.0}; @@ -142,9 +150,7 @@ NormalVehicleTracker::NormalVehicleTracker( bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; - last_input_bounding_box_ = { - bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, - bbox_object.shape.dimensions.z}; + last_input_bounding_box_ = bounding_box_; } ekf_.init(X, P); @@ -152,15 +158,28 @@ NormalVehicleTracker::NormalVehicleTracker( setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step // Set lf, lr - double point_ratio = 0.2; // under steered if smaller than 0.5 - lf_ = bounding_box_.length * point_ratio; - lr_ = bounding_box_.length * (1.0 - point_ratio); + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m } bool NormalVehicleTracker::predict(const rclcpp::Time & time) { const double dt = (time - last_update_time_).seconds(); - bool ret = predict(dt, ekf_); + if (dt < 0.0) { + RCLCPP_WARN(logger_, "dt is negative. (%f)", dt); + return false; + } + // if dt is too large, shorten dt and repeat prediction + const double dt_max = 0.11; + const uint32_t repeat = std::ceil(dt / dt_max); + const double dt_ = dt / repeat; + bool ret = false; + for (uint32_t i = 0; i < repeat; ++i) { + ret = predict(dt_, ekf_); + if (!ret) { + return false; + } + } if (ret) { last_update_time_ = time; } @@ -169,69 +188,116 @@ bool NormalVehicleTracker::predict(const rclcpp::Time & time) bool NormalVehicleTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == static bicycle model + /* Motion model: static bicycle model (constant slip angle, constant velocity) * - * x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt - * y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt - * yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt - * vx_{k+1} = vx_k + * w_k = vel_k * sin(slip_k) / l_r + * x_{k+1} = x_k + vel_k*cos(yaw_k+slip_k)*dt - 0.5*w_k*vel_k*sin(yaw_k+slip_k)*dt*dt + * y_{k+1} = y_k + vel_k*sin(yaw_k+slip_k)*dt + 0.5*w_k*vel_k*cos(yaw_k+slip_k)*dt*dt + * yaw_{k+1} = yaw_k + w_k * dt + * vel_{k+1} = vel_k * slip_{k+1} = slip_k * */ - /* == Linearized model == + /* Jacobian Matrix + * + * A = [ 1, 0, -vel*sin(yaw+slip)*dt - 0.5*w_k*vel_k*cos(yaw+slip)*dt*dt, + * cos(yaw+slip)*dt - w*sin(yaw+slip)*dt*dt, + * -vel*sin(yaw+slip)*dt - 0.5*vel*vel/l_r*(cos(slip)sin(yaw+slip)+sin(slip)cos(yaw+slip))*dt*dt ] + * [ 0, 1, vel*cos(yaw+slip)*dt - 0.5*w_k*vel_k*sin(yaw+slip)*dt*dt, + * sin(yaw+slip)*dt + w*cos(yaw+slip)*dt*dt, + * vel*cos(yaw+slip)*dt + 0.5*vel*vel/l_r*(cos(slip)cos(yaw+slip)-sin(slip)sin(yaw+slip))*dt*dt ] + * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vel/l_r*cos(slip)*dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] * - * A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt] - * [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt] - * [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt] - * [ 0, 0, 0, 1, 0] - * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP)); const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP)); + const double vel = X_t(IDX::VEL); const double cos_slip = std::cos(X_t(IDX::SLIP)); const double sin_slip = std::sin(X_t(IDX::SLIP)); - const double vx = X_t(IDX::VX); - const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 - Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state - X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw) - X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw) - X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega - X_next_t(IDX::VX) = X_t(IDX::VX); - X_next_t(IDX::SLIP) = X_t(IDX::SLIP); - - // A + double w = vel * sin_slip / lr_; + const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); + const double w_dtdt = w * dt * dt; + const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + + // Predict state vector X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state + X_next_t(IDX::X) = + X_t(IDX::X) + vel * cos_yaw * dt - 0.5 * vel * sin_slip * w_dtdt; // dx = v * cos(yaw) * dt + X_next_t(IDX::Y) = + X_t(IDX::Y) + vel * sin_yaw * dt + 0.5 * vel * cos_slip * w_dtdt; // dy = v * sin(yaw) * dt + X_next_t(IDX::YAW) = X_t(IDX::YAW) + w * dt; // d(yaw) = w * dt + X_next_t(IDX::VEL) = X_t(IDX::VEL); + X_next_t(IDX::SLIP) = X_t(IDX::SLIP); // slip_angle = asin(lr * w / v) + + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); - A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt; - A(IDX::X, IDX::VX) = cos_yaw * dt; - A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt; - A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt; - A(IDX::Y, IDX::VX) = sin_yaw * dt; - A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt; - A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt; - A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt; - - // Q + A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; + A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; + A(IDX::X, IDX::SLIP) = + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; + A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; + A(IDX::Y, IDX::SLIP) = + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; + A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; + + // Process noise covariance Q + float q_stddev_yaw_rate{0.0}; + if (vel <= 0.01) { + q_stddev_yaw_rate = ekf_params_.q_stddev_yaw_rate_min; + } else { + // uncertainty of the yaw rate is limited by + // centripetal acceleration a_lat : d(yaw)/dt = w = a_lat/v + // or maximum slip angle slip_max : w = v*sin(slip_max)/l_r + q_stddev_yaw_rate = std::min( + ekf_params_.q_stddev_acc_lat / vel, + vel * std::sin(ekf_params_.q_max_slip_angle) / lr_); // [rad/s] + q_stddev_yaw_rate = std::min(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_max); + q_stddev_yaw_rate = std::max(q_stddev_yaw_rate, ekf_params_.q_stddev_yaw_rate_min); + } + float q_cov_slip_rate{0.0f}; + if (vel <= 0.01) { + q_cov_slip_rate = ekf_params_.q_cov_slip_rate_min; + } else { + // The slip angle rate uncertainty is modeled as follows: + // d(slip)/dt ~ - sin(slip)/v * d(v)/dt + l_r/v * d(w)/dt + // where sin(slip) = w * l_r / v + // d(w)/dt is assumed to be proportional to w (more uncertain when slip is large) + // d(v)/dt and d(w)/t are considered to be uncorrelated + q_cov_slip_rate = + std::pow(ekf_params_.q_stddev_acc_lat * sin_slip / vel, 2) + std::pow(sin_slip * 1.5, 2); + q_cov_slip_rate = std::min(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_max); + q_cov_slip_rate = std::max(q_cov_slip_rate, ekf_params_.q_cov_slip_rate_min); + } + const float q_cov_x = std::pow(0.5 * ekf_params_.q_stddev_acc_long * dt * dt, 2); + const float q_cov_y = std::pow(0.5 * ekf_params_.q_stddev_acc_lat * dt * dt, 2); + const float q_cov_yaw = std::pow(q_stddev_yaw_rate * dt, 2); + const float q_cov_vel = std::pow(ekf_params_.q_stddev_acc_long * dt, 2); + const float q_cov_slip = q_cov_slip_rate * dt * dt; + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. - Q(IDX::X, IDX::X) = - (ekf_params_.q_cov_x * cos_yaw * cos_yaw + ekf_params_.q_cov_y * sin_yaw * sin_yaw) * dt * dt; - Q(IDX::X, IDX::Y) = (0.5f * (ekf_params_.q_cov_x - ekf_params_.q_cov_y) * sin_2yaw) * dt * dt; - Q(IDX::Y, IDX::Y) = - (ekf_params_.q_cov_x * sin_yaw * sin_yaw + ekf_params_.q_cov_y * cos_yaw * cos_yaw) * dt * dt; + Q(IDX::X, IDX::X) = (q_cov_x * cos_yaw * cos_yaw + q_cov_y * sin_yaw * sin_yaw); + Q(IDX::X, IDX::Y) = (0.5f * (q_cov_x - q_cov_y) * sin_2yaw); + Q(IDX::Y, IDX::Y) = (q_cov_x * sin_yaw * sin_yaw + q_cov_y * cos_yaw * cos_yaw); Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y); - Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt; - Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt; - Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt; - Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); - Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + Q(IDX::YAW, IDX::YAW) = q_cov_yaw; + Q(IDX::VEL, IDX::VEL) = q_cov_vel; + Q(IDX::SLIP, IDX::SLIP) = q_cov_slip; + + // control-input model B and control-input u are not used + // Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + // Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); if (!ekf.predict(X_next_t, A, Q)) { RCLCPP_WARN(logger_, "Cannot predict"); @@ -253,8 +319,8 @@ bool NormalVehicleTracker::measureWithPose( r_cov_x = ekf_params_.r_cov_x; r_cov_y = ekf_params_.r_cov_y; } else if (utils::isLargeVehicleLabel(label)) { - constexpr float r_stddev_x = 8.0; // [m] - constexpr float r_stddev_y = 0.8; // [m] + constexpr float r_stddev_x = 2.0; // [m] + constexpr float r_stddev_y = 2.0; // [m] r_cov_x = std::pow(r_stddev_x, 2.0); r_cov_y = std::pow(r_stddev_y, 2.0); } else { @@ -262,35 +328,25 @@ bool NormalVehicleTracker::measureWithPose( r_cov_y = ekf_params_.r_cov_y; } - // extract current state - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state - ekf_.getX(X_t); - // Decide dimension of measurement vector bool enable_velocity_measurement = false; if (object.kinematics.has_twist) { - const double predicted_vx = X_t(IDX::VX); - const double observed_vx = object.kinematics.twist_with_covariance.twist.linear.x; + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); + const double predicted_vel = X_t(IDX::VEL); + const double observed_vel = object.kinematics.twist_with_covariance.twist.linear.x; - if (std::fabs(predicted_vx - observed_vx) < velocity_deviation_threshold_) { + if (std::fabs(predicted_vel - observed_vel) < velocity_deviation_threshold_) { // Velocity deviation is small enable_velocity_measurement = true; } } - // pos x, pos y, yaw, vx depending on pose output + // pos x, pos y, yaw, vel depending on pose measurement const int dim_y = enable_velocity_measurement ? 4 : 3; - double measurement_yaw = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); - { - // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) - while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { - measurement_yaw = measurement_yaw + M_PI; - } - while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { - measurement_yaw = measurement_yaw - M_PI; - } - } + double measurement_yaw = getMeasurementYaw(object); // get sign-solved yaw angle + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf_.getX(X_t); // convert to boundingbox if input is convex shape autoware_auto_perception_msgs::msg::DetectedObject bbox_object; @@ -306,11 +362,9 @@ bool NormalVehicleTracker::measureWithPose( last_input_bounding_box_.width, last_input_bounding_box_.length, last_nearest_corner_index_, bbox_object, X_t(IDX::YAW), offset_object, tracking_offset_); - /* Set measurement matrix and noise covariance*/ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); - Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); - Y(IDX::X, 0) = offset_object.kinematics.pose_with_covariance.pose.position.x; Y(IDX::Y, 0) = offset_object.kinematics.pose_with_covariance.pose.position.y; Y(IDX::YAW, 0) = measurement_yaw; @@ -318,7 +372,8 @@ bool NormalVehicleTracker::measureWithPose( C(1, IDX::Y) = 1.0; // for pos y C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(measurement_yaw); const double sin_yaw = std::sin(measurement_yaw); @@ -342,30 +397,30 @@ bool NormalVehicleTracker::measureWithPose( // Update the velocity when necessary if (dim_y == 4) { - Y(IDX::VX, 0) = object.kinematics.twist_with_covariance.twist.linear.x; - C(3, IDX::VX) = 1.0; // for vx + Y(IDX::VEL, 0) = object.kinematics.twist_with_covariance.twist.linear.x; + C(3, IDX::VEL) = 1.0; // for vel if (!object.kinematics.has_twist_covariance) { - R(3, 3) = ekf_params_.r_cov_vx; // vx -vx + R(3, 3) = ekf_params_.r_cov_vel; // vel -vel } else { R(3, 3) = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; } } - // ekf update: this tracks tracking point + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } - // normalize yaw and limit vx, wz + // normalize yaw and limit vel, slip { Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); ekf_.getX(X_t); ekf_.getP(P_t); X_t(IDX::YAW) = tier4_autoware_utils::normalizeRadian(X_t(IDX::YAW)); - if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { - X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + if (!(-max_vel_ <= X_t(IDX::VEL) && X_t(IDX::VEL) <= max_vel_)) { + X_t(IDX::VEL) = X_t(IDX::VEL) < 0 ? -max_vel_ : max_vel_; } if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) { X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_; @@ -383,9 +438,8 @@ bool NormalVehicleTracker::measureWithPose( bool NormalVehicleTracker::measureWithShape( const autoware_auto_perception_msgs::msg::DetectedObject & object) { + // if the input shape is convex type, convert it to bbox type autoware_auto_perception_msgs::msg::DetectedObject bbox_object; - - // if input is convex shape convert it to bbox shape if (object.shape.type != autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { utils::convertConvexHullToBoundingBox(object, bbox_object); } else { @@ -400,6 +454,11 @@ bool NormalVehicleTracker::measureWithShape( gain * bounding_box_.height + (1.0 - gain) * bbox_object.shape.dimensions.z; last_input_bounding_box_ = { bbox_object.shape.dimensions.x, bbox_object.shape.dimensions.y, bbox_object.shape.dimensions.z}; + + // update lf, lr + lf_ = std::max(bounding_box_.length * 0.3, 1.0); // 30% front from the center, minimum of 1.0m + lr_ = std::max(bounding_box_.length * 0.25, 1.0); // 25% rear from the center, minimum of 1.0m + return true; } @@ -422,12 +481,6 @@ bool NormalVehicleTracker::measure( measureWithPose(object); measureWithShape(object); - // refinement - Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); - Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); - ekf_.getX(X_t); - ekf_.getP(P_t); - /* calc nearest corner index*/ setNearestCornerOrSurfaceIndex(self_transform); // this index is used in next measure step @@ -441,7 +494,7 @@ bool NormalVehicleTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -452,6 +505,7 @@ bool NormalVehicleTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -496,23 +550,44 @@ bool NormalVehicleTracker::getTrackedObject( pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::YAW, IDX::YAW); // twist - twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP)); - twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.x = X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)); + twist_with_cov.twist.linear.y = X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)); twist_with_cov.twist.angular.z = - X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k) - // twist covariance - constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)) / lr_; // yaw_rate = vel_k * sin(slip_k) / l_r + /* twist covariance + * convert covariance from velocity, slip angle to vx, vy, and yaw angle + * + * vx = vel * cos(slip) + * vy = vel * sin(slip) + * wz = vel * sin(slip) / l_r = vy / l_r + * + */ + constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; + + Eigen::MatrixXd cov_jacob(3, 2); + cov_jacob << std::cos(X_t(IDX::SLIP)), -X_t(IDX::VEL) * std::sin(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)), X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)), + std::sin(X_t(IDX::SLIP)) / lr_, X_t(IDX::VEL) * std::cos(X_t(IDX::SLIP)) / lr_; + Eigen::MatrixXd cov_twist(2, 2); + cov_twist << P(IDX::VEL, IDX::VEL), P(IDX::VEL, IDX::SLIP), P(IDX::SLIP, IDX::VEL), + P(IDX::SLIP, IDX::SLIP); + Eigen::MatrixXd twist_cov_mat = cov_jacob * cov_twist * cov_jacob.transpose(); + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = twist_cov_mat(0, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = twist_cov_mat(0, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = twist_cov_mat(0, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = twist_cov_mat(1, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = twist_cov_mat(1, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_YAW] = twist_cov_mat(1, 2); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = twist_cov_mat(2, 0); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_Y] = twist_cov_mat(2, 1); + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = twist_cov_mat(2, 2); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP); - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov; - twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP); // set shape object.shape.dimensions.x = bounding_box_.length; @@ -522,6 +597,7 @@ bool NormalVehicleTracker::getTrackedObject( const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); object.shape.footprint = tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + return true; } @@ -534,3 +610,22 @@ void NormalVehicleTracker::setNearestCornerOrSurfaceIndex( X_t(IDX::X), X_t(IDX::Y), X_t(IDX::YAW), bounding_box_.width, bounding_box_.length, self_transform); } + +double NormalVehicleTracker::getMeasurementYaw( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + double measurement_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + // Fixed measurement_yaw to be in the range of +-90 degrees of X_t(IDX::YAW) + while (M_PI_2 <= X_t(IDX::YAW) - measurement_yaw) { + measurement_yaw = measurement_yaw + M_PI; + } + while (M_PI_2 <= measurement_yaw - X_t(IDX::YAW)) { + measurement_yaw = measurement_yaw - M_PI; + } + } + return measurement_yaw; +} diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp index eed9d05359b77..d61a9a02ccd80 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -43,7 +43,7 @@ bool PedestrianAndBicycleTracker::measure( pedestrian_tracker_.measure(object, time, self_transform); bicycle_tracker_.measure(object, time, self_transform); if (object_recognition_utils::getHighestProbLabel(object.classification) != Label::UNKNOWN) - setClassification(object.classification); + updateClassification(object.classification); return true; } diff --git a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp index b168717042db3..57c2e8f899951 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -19,7 +19,6 @@ #include "multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "multi_object_tracker/utils/utils.hpp" -#include "object_recognition_utils/object_recognition_utils.hpp" #include #include @@ -35,6 +34,7 @@ #else #include #endif +#include "object_recognition_utils/object_recognition_utils.hpp" #define EIGEN_MPL2_ONLY #include @@ -52,37 +52,41 @@ PedestrianTracker::PedestrianTracker( { object_ = object; - // initialize params - float q_stddev_x = 0.4; // [m/s] - float q_stddev_y = 0.4; // [m/s] - float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] - float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float q_stddev_wz = tier4_autoware_utils::deg2rad(20); // [rad/(s*s)] - float r_stddev_x = 0.4; // [m] - float r_stddev_y = 0.4; // [m] - float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] - float p0_stddev_x = 1.0; // [m/s] - float p0_stddev_y = 1.0; // [m/s] - float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s] - float p0_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] - float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)] - ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); - ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); - ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); - ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); - ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + // Initialize parameters + // measurement noise covariance + float r_stddev_x = 0.4; // [m] + float r_stddev_y = 0.4; // [m] + float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad] ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + // initial state covariance + float p0_stddev_x = 2.0; // [m] + float p0_stddev_y = 2.0; // [m] + float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad] + float p0_stddev_vx = tier4_autoware_utils::kmph2mps(120); // [m/s] + float p0_stddev_wz = tier4_autoware_utils::deg2rad(360); // [rad/s] ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + // process noise covariance + float q_stddev_x = 0.4; // [m/s] + float q_stddev_y = 0.4; // [m/s] + float q_stddev_yaw = tier4_autoware_utils::deg2rad(20); // [rad/s] + float q_stddev_vx = tier4_autoware_utils::kmph2mps(5); // [m/(s*s)] + float q_stddev_wz = tier4_autoware_utils::deg2rad(30); // [rad/(s*s)] + ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + // limitations max_vx_ = tier4_autoware_utils::kmph2mps(10); // [m/s] max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s] - // initialize X matrix + // initialize state vector X Eigen::MatrixXd X(ekf_params_.dim_x, 1); X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; @@ -95,7 +99,7 @@ PedestrianTracker::PedestrianTracker( X(IDX::WZ) = 0.0; } - // initialize P matrix + // initialize state covariance matrix P Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); if (!object.kinematics.has_position_covariance) { const double cos_yaw = std::cos(X(IDX::YAW)); @@ -154,7 +158,7 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const { - /* == Nonlinear model == + /* Motion model: Constant turn-rate motion model (CTRV) * * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt @@ -164,7 +168,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const * */ - /* == Linearized model == + /* Jacobian Matrix * * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] @@ -173,14 +177,14 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const * [ 0, 0, 0, 0, 1] */ - // X t + // Current state vector X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state ekf.getX(X_t); const double cos_yaw = std::cos(X_t(IDX::YAW)); const double sin_yaw = std::sin(X_t(IDX::YAW)); const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); - // X t+1 + // Predict state vector X t+1 Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw) X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw) @@ -188,7 +192,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const X_next_t(IDX::VX) = X_t(IDX::VX); X_next_t(IDX::WZ) = X_t(IDX::WZ); - // A + // State transition matrix A Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt; A(IDX::X, IDX::VX) = cos_yaw * dt; @@ -196,7 +200,7 @@ bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const A(IDX::Y, IDX::VX) = sin_yaw * dt; A(IDX::YAW, IDX::WZ) = dt; - // Q + // Process noise covariance Q Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); // Rotate the covariance matrix according to the vehicle yaw // because q_cov_x and y are in the vehicle coordinate system. @@ -240,18 +244,16 @@ bool PedestrianTracker::measureWithPose( // if (tier4_autoware_utils::deg2rad(60) < std::fabs(theta)) return false; // } - /* Set measurement matrix */ + // Set measurement matrix C and observation vector Y Eigen::MatrixXd Y(dim_y, 1); + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); Y << object.kinematics.pose_with_covariance.pose.position.x, object.kinematics.pose_with_covariance.pose.position.y; - - /* Set measurement matrix */ - Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, ekf_params_.dim_x); C(0, IDX::X) = 1.0; // for pos x C(1, IDX::Y) = 1.0; // for pos y // C(2, IDX::YAW) = 1.0; // for yaw - /* Set measurement noise covariance */ + // Set noise covariance matrix R Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y); if (!object.kinematics.has_position_covariance) { R(0, 0) = ekf_params_.r_cov_x; // x - x @@ -270,6 +272,8 @@ bool PedestrianTracker::measureWithPose( // R(2, 1) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_Y]; // R(2, 2) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW]; } + + // ekf update if (!ekf_.update(Y, C, R)) { RCLCPP_WARN(logger_, "Cannot update"); } @@ -345,7 +349,7 @@ bool PedestrianTracker::getTrackedObject( object.object_id = getUUID(); object.classification = getClassification(); - // predict kinematics + // predict state KalmanFilter tmp_ekf_for_no_update = ekf_; const double dt = (time - last_update_time_).seconds(); if (0.001 /*1msec*/ < dt) { @@ -356,6 +360,7 @@ bool PedestrianTracker::getTrackedObject( tmp_ekf_for_no_update.getX(X_t); tmp_ekf_for_no_update.getP(P); + /* put predicted pose and twist to output object */ auto & pose_with_cov = object.kinematics.pose_with_covariance; auto & twist_with_cov = object.kinematics.twist_with_covariance; @@ -399,6 +404,7 @@ bool PedestrianTracker::getTrackedObject( constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wx_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative constexpr double wy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; diff --git a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp index ba684e4777947..a3320ff54afcb 100644 --- a/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp +++ b/perception/multi_object_tracker/src/tracker/model/tracker_base.cpp @@ -54,6 +54,67 @@ bool Tracker::updateWithoutMeasurement() return true; } +void Tracker::updateClassification( + const std::vector & classification) +{ + // classification algorithm: + // 0. Normalize the input classification + // 1-1. Update the matched classification probability with a gain (ratio of 0.05) + // 1-2. If the label is not found, add it to the classification list + // 2. Remove the class with probability < remove_threshold (0.001) + // 3. Normalize tracking classification + + // Parameters + // if the remove_threshold is too high (compare to the gain), the classification will be removed + // immediately + const double gain = 0.05; + constexpr double remove_threshold = 0.001; + + // Normalization function + auto normalizeProbabilities = + [](std::vector & classification) { + double sum = 0.0; + for (const auto & class_ : classification) { + sum += class_.probability; + } + for (auto & class_ : classification) { + class_.probability /= sum; + } + }; + + // Normalize the input + auto classification_input = classification; + normalizeProbabilities(classification_input); + + // Update the matched classification probability with a gain + for (const auto & new_class : classification_input) { + bool found = false; + for (auto & old_class : classification_) { + if (new_class.label == old_class.label) { + old_class.probability += new_class.probability * gain; + found = true; + break; + } + } + // If the label is not found, add it to the classification list + if (!found) { + auto adding_class = new_class; + adding_class.probability *= gain; + classification_.push_back(adding_class); + } + } + + // If the probability is less than the threshold, remove the class + classification_.erase( + std::remove_if( + classification_.begin(), classification_.end(), + [remove_threshold](const auto & class_) { return class_.probability < remove_threshold; }), + classification_.end()); + + // Normalize tracking classification + normalizeProbabilities(classification_); +} + geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( const rclcpp::Time & time) const { diff --git a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp index 691c13a6d4701..78f76d4f980c3 100644 --- a/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp +++ b/perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp @@ -239,19 +239,42 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent( void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack( const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc) { - PclPointCloud tmp_behind_pc; - PclPointCloud tmp_front_pc; - for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"), y(*input_pc, "y"), - z(*input_pc, "z"); - x != x.end(); ++x, ++y, ++z) { + size_t front_count = 0; + size_t behind_count = 0; + + for (sensor_msgs::PointCloud2ConstIterator x(*input_pc, "x"); x != x.end(); ++x) { if (*x < 0.0) { - tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + behind_count++; } else { - tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z)); + front_count++; } } - pcl::toROSMsg(tmp_front_pc, front_pc); - pcl::toROSMsg(tmp_behind_pc, behind_pc); + + sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc); + sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc); + front_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz"); + front_pc_modifier.resize(front_count); + behind_pc_modifier.resize(behind_count); + + sensor_msgs::PointCloud2Iterator fr_iter(front_pc, "x"); + sensor_msgs::PointCloud2Iterator be_iter(behind_pc, "x"); + + for (sensor_msgs::PointCloud2ConstIterator in_iter(*input_pc, "x"); + in_iter != in_iter.end(); ++in_iter) { + if (*in_iter < 0.0) { + *be_iter = in_iter[0]; + be_iter[1] = in_iter[1]; + be_iter[2] = in_iter[2]; + ++be_iter; + } else { + *fr_iter = in_iter[0]; + fr_iter[1] = in_iter[1]; + fr_iter[2] = in_iter[2]; + ++fr_iter; + } + } + front_pc.header = input_pc->header; behind_pc.header = input_pc->header; } diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 962bcdc6f154f..3fbace69bc3b8 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -70,3 +70,39 @@ Additional argument is shown below: | `container_name` | `occupancy_grid_map_container` | | | `input_obstacle_pointcloud` | `false` | only for laserscan based method. If true, the node subscribe obstacle pointcloud | | `input_obstacle_and_raw_pointcloud` | `true` | only for laserscan based method. If true, the node subscribe both obstacle and raw pointcloud | + +### Downsample input pointcloud(Optional) + +If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used. This feature is currently only for the pointcloud based occupancy grid map. + +- pointcloud_based_occupancy_grid_map method + +```yaml +# downsampled raw and obstacle pointcloud +/perception/occupancy_grid_map/obstacle/downsample/pointcloud +/perception/occupancy_grid_map/raw/downsample/pointcloud +``` + +- multi_lidar_pointcloud_based_point_cloud + +```yaml +# downsampled raw and obstacle pointcloud +/perception/occupancy_grid_map/obstacle/downsample/pointcloud +/perception/occupancy_grid_map//raw/downsample/pointcloud +``` + +### Test + +This package provides unit tests using `gtest`. +You can run the test by the following command. + +```bash +colcon test --packages-select probabilistic_occupancy_grid_map --event-handlers console_direct+ +``` + +Test contains the following. + +- Unit test for cost value conversion function +- Unit test for utility functions +- Unit test for occupancy grid map fusion functions +- Input/Output test for pointcloud based occupancy grid map diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml index 8077bdec5008e..922d4be6d519f 100644 --- a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml @@ -8,6 +8,10 @@ min_height: -1.0 max_height: 2.0 + # downsample input pointcloud + downsample_input_pointcloud: true + downsample_voxel_size: 0.25 # [m] + enable_single_frame_mode: false # use sensor pointcloud to filter obstacle pointcloud filter_obstacle_pointcloud_by_raw_pointcloud: false diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index 49bf228905dcc..8584d81a7de57 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -26,6 +26,47 @@ import yaml +def get_downsample_filter_node(setting: dict) -> ComposableNode: + plugin_str = setting["plugin"] + voxel_size = setting["voxel_size"] + node_name = setting["node_name"] + return ComposableNode( + package="pointcloud_preprocessor", + plugin=plugin_str, + name=node_name, + remappings=[ + ("input", setting["input_topic"]), + ("output", setting["output_topic"]), + ], + parameters=[ + { + "voxel_size_x": voxel_size, + "voxel_size_y": voxel_size, + "voxel_size_z": voxel_size, + } + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + +def get_downsample_preprocess_nodes(voxel_size: float) -> list: + raw_settings = { + "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", + "node_name": "raw_pc_downsample_filter", + "input_topic": LaunchConfiguration("input/raw_pointcloud"), + "output_topic": "raw/downsample/pointcloud", + "voxel_size": voxel_size, + } + obstacle_settings = { + "plugin": "pointcloud_preprocessor::PickupBasedVoxelGridDownsampleFilterComponent", + "node_name": "obstacle_pc_downsample_filter", + "input_topic": LaunchConfiguration("input/obstacle_pointcloud"), + "output_topic": "obstacle/downsample/pointcloud", + "voxel_size": voxel_size, + } + return [get_downsample_filter_node(raw_settings), get_downsample_filter_node(obstacle_settings)] + + def launch_setup(context, *args, **kwargs): # load parameter files param_file = LaunchConfiguration("param_file").perform(context) @@ -38,7 +79,21 @@ def launch_setup(context, *args, **kwargs): with open(updater_param_file, "r") as f: occupancy_grid_map_updater_params = yaml.safe_load(f)["/**"]["ros__parameters"] - composable_nodes = [ + # composable nodes + composable_nodes = [] + + # add downsample process + downsample_input_pointcloud: bool = pointcloud_based_occupancy_grid_map_node_params[ + "downsample_input_pointcloud" + ] + if downsample_input_pointcloud: + voxel_grid_size: float = pointcloud_based_occupancy_grid_map_node_params[ + "downsample_voxel_size" + ] + downsample_preprocess_nodes = get_downsample_preprocess_nodes(voxel_grid_size) + composable_nodes.extend(downsample_preprocess_nodes) + + composable_nodes.append( ComposableNode( package="probabilistic_occupancy_grid_map", plugin="occupancy_grid_map::PointcloudBasedOccupancyGridMapNode", @@ -46,11 +101,15 @@ def launch_setup(context, *args, **kwargs): remappings=[ ( "~/input/obstacle_pointcloud", - LaunchConfiguration("input/obstacle_pointcloud"), + LaunchConfiguration("input/obstacle_pointcloud") + if not downsample_input_pointcloud + else "obstacle/downsample/pointcloud", ), ( "~/input/raw_pointcloud", - LaunchConfiguration("input/raw_pointcloud"), + LaunchConfiguration("input/raw_pointcloud") + if not downsample_input_pointcloud + else "raw/downsample/pointcloud", ), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], @@ -60,8 +119,8 @@ def launch_setup(context, *args, **kwargs): {"updater_type": LaunchConfiguration("updater_type")}, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], - ), - ] + ) + ) occupancy_grid_map_container = ComposableNodeContainer( name=LaunchConfiguration("container_name"), diff --git a/perception/probabilistic_occupancy_grid_map/package.xml b/perception/probabilistic_occupancy_grid_map/package.xml index a90ae78b03597..3efed07535504 100644 --- a/perception/probabilistic_occupancy_grid_map/package.xml +++ b/perception/probabilistic_occupancy_grid_map/package.xml @@ -34,6 +34,7 @@ tier4_autoware_utils visualization_msgs + pointcloud_preprocessor pointcloud_to_laserscan ament_cmake_gtest @@ -44,6 +45,7 @@ launch_testing launch_testing_ament_cmake launch_testing_ros + pointcloud_preprocessor ament_cmake diff --git a/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md b/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md index 7d2efb3468f3a..d8d69ed7866b6 100644 --- a/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md +++ b/perception/probabilistic_occupancy_grid_map/pointcloud-based-occupancy-grid-map.md @@ -77,18 +77,34 @@ $$ | ----------------------------- | ------------------------- | ------------------ | | `~/output/occupancy_grid_map` | `nav_msgs::OccupancyGrid` | occupancy grid map | +### Related topics + +If you set `downsample_input_pointcloud` to `true`, the input pointcloud will be downsampled and following topics are also used. + +- pointcloud_based_occupancy_grid_map method + +```yaml +# downsampled raw and obstacle pointcloud +/perception/occupancy_grid_map/obstacle/downsample/pointcloud +/perception/occupancy_grid_map/raw/downsample/pointcloud +``` + ## Parameters ### Node Parameters -| Name | Type | Description | -| ------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------- | -| `map_frame` | string | map frame | -| `base_link_frame` | string | base_link frame | -| `use_height_filter` | bool | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. | -| `map_length` | double | The length of the map. -100 if it is 50~50[m] | -| `map_resolution` | double | The map cell resolution [m] | -| `grid_map_type` | string | The type of grid map for estimating `UNKNOWN` region behind obstacle point clouds | +| Name | Type | Description | +| ----------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------- | +| `map_frame` | string | map frame | +| `base_link_frame` | string | base_link frame | +| `use_height_filter` | bool | whether to height filter for `~/input/obstacle_pointcloud` and `~/input/raw_pointcloud`? By default, the height is set to -1~2m. | +| `map_length` | double | The length of the map. -100 if it is 50~50[m] | +| `map_resolution` | double | The map cell resolution [m] | +| `grid_map_type` | string | The type of grid map for estimating `UNKNOWN` region behind obstacle point clouds | +| `scan_origin` | string | The origin of the scan. It should be a sensor frame. | +| `pub_debug_grid` | bool | Whether to publish debug grid maps | +| `downsample_input_pointcloud` | bool | Whether to downsample the input pointclouds. The downsampled pointclouds are used for the ray tracing. | +| `downsample_voxel_size` | double | The voxel size for the downsampled pointclouds. | ## Assumptions / Known limits diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml index 702809b3cede1..c232dbde40b51 100644 --- a/perception/tracking_object_merger/config/data_association_matrix.param.yaml +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -3,7 +3,7 @@ lidar-lidar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -59,7 +59,7 @@ lidar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS @@ -115,7 +115,7 @@ radar-radar: can_assign_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN - [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN 0, 1, 1, 1, 1, 0, 0, 0, #CAR 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK 0, 1, 1, 1, 1, 0, 0, 0, #BUS diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp index 81f63538f9e22..4901c2fe09f9c 100644 --- a/perception/tracking_object_merger/src/utils/utils.cpp +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -324,43 +324,61 @@ autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMe // copy main object at first output_kinematics = main_obj.kinematics; auto sub_obj_ = sub_obj; - // do not merge reverse direction objects + // do not merge if motion direction is different if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { return output_kinematics; - } else if (objectsYawIsReverted(main_obj, sub_obj)) { - // revert velocity (revert pose is not necessary because it is not used in tracking) - sub_obj_.kinematics.twist_with_covariance.twist.linear.x *= -1.0; - sub_obj_.kinematics.twist_with_covariance.twist.linear.y *= -1.0; } // currently only merge vx if (policy == MergePolicy::SKIP) { return output_kinematics; } else if (policy == MergePolicy::OVERWRITE) { - output_kinematics.twist_with_covariance.twist.linear.x = - sub_obj_.kinematics.twist_with_covariance.twist.linear.x; + // use main_obj's orientation + // take sub_obj's velocity vector and convert into main_obj's frame, but take only x component + const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y; + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation); + const auto sub_vx_in_main_frame = + sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw); + output_kinematics.twist_with_covariance.twist.linear.x = sub_vx_in_main_frame; + return output_kinematics; } else if (policy == MergePolicy::FUSION) { + // use main_obj's orientation + // take main_obj's velocity vector and convert into sub_obj's frame, but take only x component const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; const auto sub_vx = sub_obj_.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vy = sub_obj_.kinematics.twist_with_covariance.twist.linear.y; + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj_.kinematics.pose_with_covariance.pose.orientation); + const auto sub_vel_in_main_frame_x = + sub_vx * std::cos(sub_yaw - main_yaw) + sub_vy * std::sin(sub_yaw - main_yaw); // inverse weight const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; const auto sub_vx_cov = sub_obj_.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vy_cov = sub_obj_.kinematics.twist_with_covariance.covariance[7]; + const auto sub_vel_cov_in_main_frame_x = + sub_vx_cov * std::cos(sub_yaw - main_yaw) * std::cos(sub_yaw - main_yaw) + + sub_vy_cov * std::sin(sub_yaw - main_yaw) * std::sin(sub_yaw - main_yaw); double main_vx_weight, sub_vx_weight; if (main_vx_cov == 0.0) { main_vx_weight = 1.0 * 1e6; } else { main_vx_weight = 1.0 / main_vx_cov; } - if (sub_vx_cov == 0.0) { + if (sub_vel_cov_in_main_frame_x == 0.0) { sub_vx_weight = 1.0 * 1e6; } else { - sub_vx_weight = 1.0 / sub_vx_cov; + sub_vx_weight = 1.0 / sub_vel_cov_in_main_frame_x; } - // merge with covariance + + // merge velocity with covariance output_kinematics.twist_with_covariance.twist.linear.x = - (main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight); + (main_vx * main_vx_weight + sub_vel_in_main_frame_x * sub_vx_weight) / + (main_vx_weight + sub_vx_weight); output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight); + return output_kinematics; } else { std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl; @@ -453,21 +471,25 @@ autoware_auto_perception_msgs::msg::Shape shapeMerger( void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) { + // do not update if motion direction is different if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { - // do not update velocity - return; - } else if (objectsYawIsReverted(main_obj, sub_obj)) { - // revert velocity (revert pose is not necessary because it is not used in tracking) - const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; - main_obj = sub_obj; - main_obj.kinematics.twist_with_covariance.twist.linear.x = -vx_temp; return; - } else { - // update velocity - const auto vx_temp = sub_obj.kinematics.twist_with_covariance.twist.linear.x; - main_obj = sub_obj; - main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; } + // take main_obj's velocity vector and convert into sub_obj's frame + // use sub_obj's orientation, but take only x component + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto main_vy = main_obj.kinematics.twist_with_covariance.twist.linear.y; + const auto main_yaw = tf2::getYaw(main_obj.kinematics.pose_with_covariance.pose.orientation); + const auto sub_yaw = tf2::getYaw(sub_obj.kinematics.pose_with_covariance.pose.orientation); + const auto main_vx_in_sub_frame = + main_vx * std::cos(main_yaw - sub_yaw) + main_vy * std::sin(main_yaw - sub_yaw); + + // copy sub object to fused object + main_obj = sub_obj; + // recover main object's velocity + main_obj.kinematics.twist_with_covariance.twist.linear.x = main_vx_in_sub_frame; + + return; } void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) @@ -484,9 +506,9 @@ void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & su { if (!objectsHaveSameMotionDirections(main_obj, sub_obj)) { // warning - std::cerr << "[object_tracking_merger]: motion direction is different in " - "updateWholeTrackedObject function." - << std::endl; + // std::cerr << "[object_tracking_merger]: motion direction is different in " + // "updateWholeTrackedObject function." + // << std::endl; } main_obj = sub_obj; } diff --git a/perception/traffic_light_arbiter/CMakeLists.txt b/perception/traffic_light_arbiter/CMakeLists.txt index 181c2bec867d1..a350bdcf73cba 100644 --- a/perception/traffic_light_arbiter/CMakeLists.txt +++ b/perception/traffic_light_arbiter/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/traffic_light_arbiter.cpp + src/signal_match_validator.cpp ) rclcpp_components_register_nodes(${PROJECT_NAME} diff --git a/perception/traffic_light_arbiter/README.md b/perception/traffic_light_arbiter/README.md index 952119020c215..ad25a6514ea1f 100644 --- a/perception/traffic_light_arbiter/README.md +++ b/perception/traffic_light_arbiter/README.md @@ -8,6 +8,19 @@ This package receives traffic signals from perception and external (e.g., V2X) c A node that merges traffic light/signal state from image recognition and external (e.g., V2X) systems to provide to a planning component. +### Signal Match Validator + +When `enable_signal_matching` is set to true, this node validates the match between perception signals and external signals. +The table below outlines how the matching process determines the output based on the combination of perception and external signal colors. Each cell represents the outcome when a specific color from a perception signal (columns) intersects with a color from an external signal (rows). + +| External \ Perception | RED | AMBER | GREEN | UNKNOWN | Not Received | +| --------------------- | ------- | ------- | ------- | ------- | ------------ | +| RED | RED | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | +| AMBER | UNKNOWN | AMBER | UNKNOWN | UNKNOWN | UNKNOWN | +| GREEN | UNKNOWN | UNKNOWN | GREEN | UNKNOWN | UNKNOWN | +| UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | +| Not Received | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | UNKNOWN | + ### Inputs / Outputs #### Input @@ -28,8 +41,9 @@ A node that merges traffic light/signal state from image recognition and externa ### Core Parameters -| Name | Type | Default Value | Description | -| --------------------------- | ------ | ------------- | -------------------------------------------------------------------------------------------------------------------------------- | -| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging | -| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging | -| `external_priority` | bool | false | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria | +| Name | Type | Default Value | Description | +| --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging | +| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging | +| `external_priority` | bool | false | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria | +| `enable_signal_matching` | bool | false | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical | diff --git a/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml index 5dc2b62eaa446..dfe12ff352f16 100644 --- a/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml +++ b/perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml @@ -3,3 +3,4 @@ external_time_tolerance: 5.0 perception_time_tolerance: 1.0 external_priority: false + enable_signal_matching: false diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp new file mode 100644 index 0000000000000..68c9fb5ce9964 --- /dev/null +++ b/perception/traffic_light_arbiter/include/traffic_light_arbiter/signal_match_validator.hpp @@ -0,0 +1,103 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ +#define TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +/** + * @class SignalMatchValidator + * @brief Validates and compares traffic signal data from different sources. + * + * This class is designed to compare and validate traffic signal information + * received from internal (e.g. camera perception) and external (e.g. V2I communication). + * It aims to ensure that the traffic signal information from both sources is consistent. + */ +class SignalMatchValidator +{ +public: + using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; + using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficLightConstPtr = lanelet::TrafficLightConstPtr; + + /** + * @brief Default constructor for SignalMatchValidator. + */ + SignalMatchValidator() = default; + + /** + * @brief Validates and compares traffic signal data from perception and external sources. + * + * This method compares traffic signal data obtained from perception results + * with data received from external source. It aims to find and return signals + * that are present and consistent in both datasets. If signals are not consistent, + * treat them as the unknown signal. + * + * @param perception_signals Traffic signal data from perception. + * @param external_signals Traffic signal data from external source. + * @return A validated TrafficSignalArray. + */ + TrafficSignalArray validateSignals( + const TrafficSignalArray & perception_signals, const TrafficSignalArray & external_signals); + + /** + * @brief Sets the pedestrian signals to be considered during validation. + * + * This method allows the specification of pedestrian signals, which are then + * used to adjust the validation logic, acknowledging their unique characteristics + * in traffic signal datasets. + * + * @param pedestrian_signals A vector of pedestrian signal pointers. + */ + void setPedestrianSignals(const std::vector & pedestrian_signals); + + /** + * @brief Sets the priority flag for using external signal data over perception data. + * + * When set to true, this flag indicates that in cases of discrepancy between + * perception and external signal data, the external data should be prioritized. + * + * @param external_priority The priority flag for external signal data. + */ + void setExternalPriority(const bool external_priority); + +private: + bool external_priority_; + std::unordered_set map_pedestrian_signal_regulatory_elements_set_; + + /** + * @brief Checks if a given signal ID corresponds to a pedestrian signal. + * + * This method determines whether a signal, identified by its ID, is classified + * as a pedestrian signal. + * + * @param signal_id The ID of the signal to check. + * @return True if the signal is a pedestrian signal, false otherwise. + */ + bool isPedestrianSignal(const lanelet::Id & signal_id); +}; + +#endif // TRAFFIC_LIGHT_ARBITER__SIGNAL_MATCH_VALIDATOR_HPP_ diff --git a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp index 485ce84c5fea6..155e5e619062e 100644 --- a/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/traffic_light_arbiter/include/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -16,6 +16,7 @@ #define TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ #include +#include #include #include @@ -51,9 +52,11 @@ class TrafficLightArbiter : public rclcpp::Node double external_time_tolerance_; double perception_time_tolerance_; bool external_priority_; + bool enable_signal_matching_; TrafficSignalArray latest_perception_msg_; TrafficSignalArray latest_external_msg_; + std::unique_ptr signal_match_validator_; }; #endif // TRAFFIC_LIGHT_ARBITER__TRAFFIC_LIGHT_ARBITER_HPP_ diff --git a/perception/traffic_light_arbiter/src/signal_match_validator.cpp b/perception/traffic_light_arbiter/src/signal_match_validator.cpp new file mode 100644 index 0000000000000..312f2e596a493 --- /dev/null +++ b/perception/traffic_light_arbiter/src/signal_match_validator.cpp @@ -0,0 +1,287 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "traffic_light_arbiter/signal_match_validator.hpp" + +namespace util +{ +using TrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; +using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; +using Element = autoware_perception_msgs::msg::TrafficSignalElement; +using Time = builtin_interfaces::msg::Time; + +// Finds a signal by its ID within a TrafficSignalArray +std::optional find_signal_by_id( + const std::unordered_map & id_signal_map, int64_t signal_id) +{ + auto it = id_signal_map.find(signal_id); + if (it != id_signal_map.end()) { + return it->second; // Return the found signal + } else { + return std::nullopt; // Return an empty optional if not found + } +} + +// Creates a map from signal IDs to TrafficSignal objects. +std::unordered_map create_id_signal_map( + const TrafficSignalArray & traffic_signals) +{ + std::unordered_map id_signal_map; + for (const auto & traffic_signal : traffic_signals.signals) { + id_signal_map[traffic_signal.traffic_signal_id] = traffic_signal; + } + + return id_signal_map; +} + +// Creates a TrafficSignalElement with specified attributes +Element create_element( + const Element::_color_type & color, const Element::_shape_type & shape, + const Element::_status_type & status, const Element::_confidence_type & confidence) +{ + Element signal_element; + signal_element.color = color; + signal_element.shape = shape; + signal_element.status = status; + signal_element.confidence = confidence; + + return signal_element; +} + +// Creates unknown elements for each unique shape from two element vectors +std::vector create_unknown_elements( + const std::vector & elements1, const std::vector & elements2) +{ + std::unordered_set shape_set; + for (const auto & element : elements1) { + shape_set.emplace(element.shape); + } + for (const auto & element : elements2) { + shape_set.emplace(element.shape); + } + + std::vector unknown_elements; + for (const auto & shape : shape_set) { + // Confidence is set to a default value as it is not relevant for unknown signals + unknown_elements.emplace_back( + create_element(Element::UNKNOWN, shape, Element::UNKNOWN, /* confidence */ 1.0)); + } + + return unknown_elements; +} + +// Creates a 'unknown' signal with elements matching the shapes of a given signal's elements +TrafficSignal create_unknown_signal(const TrafficSignal & traffic_signal) +{ + TrafficSignal unknown_signal; + unknown_signal.traffic_signal_id = traffic_signal.traffic_signal_id; + for (const auto & element : traffic_signal.elements) { + // Confidence is set to a default value as it is not relevant for unknown signals + const auto unknown_element = + create_element(Element::UNKNOWN, element.shape, Element::UNKNOWN, /* confidence */ 1.0); + unknown_signal.elements.emplace_back(unknown_element); + } + + return unknown_signal; +} + +// Creates an 'unknown' signal by combining unique shapes from two signals' elements +TrafficSignal create_unknown_signal(const TrafficSignal & signal1, const TrafficSignal & signal2) +{ + TrafficSignal unknown_signal; + + // Assumes that both signals have the same traffic_signal_id + unknown_signal.traffic_signal_id = signal1.traffic_signal_id; + + const auto unknown_elements = create_unknown_elements(signal1.elements, signal2.elements); + for (const auto & element : unknown_elements) { + unknown_signal.elements.emplace_back(element); + } + + return unknown_signal; +} + +// Checks if all elements in two vectors are equivalent +bool are_all_elements_equivalent( + const std::vector & signal1, const std::vector & signal2) +{ + // Returns false if vectors have different sizes + if (signal1.size() != signal2.size()) { + return false; + } + + // Sorts copies of the vectors by shape for comparison + std::vector sorted_signal1 = signal1; + std::vector sorted_signal2 = signal2; + auto compare_by_shape = [](const Element & a, const Element & b) { return a.shape < b.shape; }; + std::sort(sorted_signal1.begin(), sorted_signal1.end(), compare_by_shape); + std::sort(sorted_signal2.begin(), sorted_signal2.end(), compare_by_shape); + + // Returns true if sorted vectors are equal + return std::equal( + sorted_signal1.begin(), sorted_signal1.end(), sorted_signal2.begin(), sorted_signal2.end(), + [](const Element & a, const Element & b) { return a.color == b.color && a.shape == b.shape; }); +} + +// Creates a set of unique signal IDs from two vectors of TrafficSignals +std::unordered_set create_signal_id_set( + const std::vector & signals1, const std::vector & signals2) +{ + std::unordered_set signal_id_set; + for (const auto & traffic_signal : signals1) { + signal_id_set.emplace(traffic_signal.traffic_signal_id); + } + for (const auto & traffic_signal : signals2) { + signal_id_set.emplace(traffic_signal.traffic_signal_id); + } + + return signal_id_set; +} + +// Returns the signal with the highest confidence elements, considering a external priority +TrafficSignal get_highest_confidence_signal( + const std::optional & perception_signal, + const std::optional & external_signal, const bool external_priority) +{ + // Returns the existing signal if only one of them exists + if (!perception_signal) { + return *external_signal; + } + if (!external_signal) { + return *perception_signal; + } + + // Gives priority to the external signal if external_priority is true + if (external_priority) { + return *external_signal; + } + + // Compiles elements into a map by shape, to compare their confidences + using Key = Element::_shape_type; + std::map> shape_element_map; + for (const auto & element : perception_signal->elements) { + shape_element_map[element.shape].emplace_back(element); + } + for (const auto & element : external_signal->elements) { + shape_element_map[element.shape].emplace_back(element); + } + + TrafficSignal highest_confidence_signal; + + // Assumes that both signals have the same traffic_signal_id + highest_confidence_signal.traffic_signal_id = perception_signal->traffic_signal_id; + + // For each shape, finds the element with the highest confidence and adds it to the signal + for (const auto & [shape, elements] : shape_element_map) { + const auto highest_confidence_element = std::max_element( + elements.begin(), elements.end(), + [](const Element & a, const Element & b) { return a.confidence < b.confidence; }); + highest_confidence_signal.elements.emplace_back(*highest_confidence_element); + } + + return highest_confidence_signal; +} + +// Determines the newer of two Time stamps +Time get_newer_stamp(const Time & stamp1, const Time & stamp2) +{ + // Returns stamp1 if it is newer than stamp2, otherwise returns stamp2 + if (stamp1.sec > stamp2.sec || (stamp1.sec == stamp2.sec && stamp1.nanosec > stamp2.nanosec)) { + return stamp1; + } else { + return stamp2; + } +} + +} // namespace util + +autoware_perception_msgs::msg::TrafficSignalArray SignalMatchValidator::validateSignals( + const TrafficSignalArray & perception_signals, const TrafficSignalArray & external_signals) +{ + TrafficSignalArray validated_signals; + + // Set newer stamp + validated_signals.stamp = util::get_newer_stamp(perception_signals.stamp, external_signals.stamp); + + // Create a map from signals to reduce the calculation cost + const auto perception_id_signal_map = util::create_id_signal_map(perception_signals); + const auto external_id_signal_map = util::create_id_signal_map(external_signals); + + // Create the unique set of the received id, + // then compare the signal element for each received signal id + const auto received_signal_id_set = + util::create_signal_id_set(perception_signals.signals, external_signals.signals); + + for (const auto & signal_id : received_signal_id_set) { + const auto perception_result = util::find_signal_by_id(perception_id_signal_map, signal_id); + const auto external_result = util::find_signal_by_id(external_id_signal_map, signal_id); + // Neither result exists + if (!perception_result && !external_result) { + continue; + } + + // We don't validate the pedestrian signals + // TODO(TomohitoAndo): Validate pedestrian signals + if (isPedestrianSignal(signal_id)) { + validated_signals.signals.emplace_back(util::get_highest_confidence_signal( + perception_result, external_result, external_priority_)); + + continue; + } + + // If either of the signal is not received, treat as unknown signal + if (!perception_result && external_result) { + const auto unknown_signal = util::create_unknown_signal(*external_result); + validated_signals.signals.emplace_back(unknown_signal); + continue; + } + if (!external_result && perception_result) { + const auto unknown_signal = util::create_unknown_signal(*perception_result); + validated_signals.signals.emplace_back(unknown_signal); + continue; + } + + // Check if they have the same elements + if (!util::are_all_elements_equivalent( + perception_result->elements, external_result->elements)) { + const auto unknown_signal = util::create_unknown_signal(*perception_result, *external_result); + validated_signals.signals.emplace_back(unknown_signal); + continue; + } + + // Both results are same, then insert the received color + validated_signals.signals.emplace_back(*perception_result); + } + + return validated_signals; +} + +void SignalMatchValidator::setPedestrianSignals( + const std::vector & pedestrian_signals) +{ + for (const auto & pedestrian_signal : pedestrian_signals) { + map_pedestrian_signal_regulatory_elements_set_.emplace(pedestrian_signal->id()); + } +} + +void SignalMatchValidator::setExternalPriority(const bool external_priority) +{ + external_priority_ = external_priority; +} + +bool SignalMatchValidator::isPedestrianSignal(const lanelet::Id & signal_id) +{ + return map_pedestrian_signal_regulatory_elements_set_.find(signal_id) != + map_pedestrian_signal_regulatory_elements_set_.end(); +} diff --git a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp index 3838e93113066..f9a1888139071 100644 --- a/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -40,6 +41,25 @@ std::vector filter_traffic_signals(const LaneletMapConstPt signals.push_back(signal); } } + + return signals; +} + +std::vector filter_pedestrian_signals(const LaneletMapConstPtr map) +{ + namespace query = lanelet::utils::query; + + const auto all_lanelets = query::laneletLayer(map); + const auto crosswalks = query::crosswalkLanelets(all_lanelets); + std::vector signals; + + for (const auto & crosswalk : crosswalks) { + const auto traffic_light_reg_elems = + crosswalk.regulatoryElementsAs(); + std::copy( + traffic_light_reg_elems.begin(), traffic_light_reg_elems.end(), std::back_inserter(signals)); + } + return signals; } @@ -51,6 +71,12 @@ TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) external_time_tolerance_ = this->declare_parameter("external_time_tolerance", 5.0); perception_time_tolerance_ = this->declare_parameter("perception_time_tolerance", 1.0); external_priority_ = this->declare_parameter("external_priority", false); + enable_signal_matching_ = this->declare_parameter("enable_signal_matching", false); + + if (enable_signal_matching_) { + signal_match_validator_ = std::make_unique(); + signal_match_validator_->setExternalPriority(external_priority_); + } map_sub_ = create_subscription( "~/sub/vector_map", rclcpp::QoS(1).transient_local(), @@ -78,6 +104,12 @@ void TrafficLightArbiter::onMap(const LaneletMapBin::ConstSharedPtr msg) for (const auto & signal : signals) { map_regulatory_elements_set_->emplace(signal->id()); } + + if (enable_signal_matching_) { + // Filter only pedestrian signals to distinguish them in signal matching + const auto pedestrian_signals = lanelet::filter_pedestrian_signals(map); + signal_match_validator_->setPedestrianSignals(pedestrian_signals); + } } void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedPtr msg) @@ -140,12 +172,20 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim } }; - for (const auto & signal : latest_perception_msg_.signals) { - add_signal_function(signal, false); - } + if (enable_signal_matching_) { + const auto validated_signals = + signal_match_validator_->validateSignals(latest_perception_msg_, latest_external_msg_); + for (const auto & signal : validated_signals.signals) { + add_signal_function(signal, false); + } + } else { + for (const auto & signal : latest_perception_msg_.signals) { + add_signal_function(signal, false); + } - for (const auto & signal : latest_external_msg_.signals) { - add_signal_function(signal, external_priority_); + for (const auto & signal : latest_external_msg_.signals) { + add_signal_function(signal, external_priority_); + } } const auto get_highest_confidence_elements = diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml index 3e8907cdccdf6..ad5fe0b123f1d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml @@ -12,64 +12,80 @@ moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] truck: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bus: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] trailer: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] bicycle: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] motorcycle: execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] pedestrian: execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + lateral_margin: + soft_margin: 0.0 # [m] + hard_margin: 0.0 # [m] + hard_margin_for_parked_vehicle: 0.0 # [m] lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index 3a4f42f2d2a6c..0f9f3f6dc9d42 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -22,6 +22,7 @@ behavior_path_lane_change_module behavior_path_planner behavior_path_planner_common + lanelet2_extension motion_utils pluginlib rclcpp diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 3f810710ef37b..c53c272bcb6fa 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); return param; }; @@ -130,11 +132,14 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) } { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); + const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 619cbc515f816..efd169c937abc 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include #include #include @@ -29,16 +30,6 @@ namespace behavior_path_planner { -namespace -{ -lanelet::BasicLineString3d toLineString3d(const std::vector & bound) -{ - lanelet::BasicLineString3d ret{}; - std::for_each( - bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); - return ret; -} -} // namespace AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) @@ -95,9 +86,11 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); const auto nearest_object_parameter = avoidance_parameters_->object_parameters.at(nearest_object_type); - const auto avoid_margin = - nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + - nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; + const auto lateral_hard_margin = std::max( + nearest_object_parameter.lateral_hard_margin, + nearest_object_parameter.lateral_hard_margin_for_parked_vehicle); + const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor + + nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width; avoidance_helper_->setData(planner_data_); const auto shift_length = avoidance_helper_->getShiftLength( @@ -161,19 +154,17 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( // expand drivable lanes std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back(utils::avoidance::generateExpandDrivableLanes( + data.drivable_lanes.push_back(utils::avoidance::generateExpandedDrivableLanes( lanelet, planner_data_, avoidance_parameters_)); }); // calc drivable bound const auto shorten_lanes = utils::cutOverlappedLanes(data.reference_path_rough, data.drivable_lanes); - data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, true)); - data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, avoidance_parameters_->use_hatched_road_markings, - avoidance_parameters_->use_intersection_areas, false)); + data.left_bound = utils::calcBound( + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, true); + data.right_bound = utils::calcBound( + data.reference_path_rough, planner_data_, shorten_lanes, false, false, false, false); // get related objects from dynamic_objects, and then separates them as target objects and non // target objects @@ -183,7 +174,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( } void AvoidanceByLaneChange::fillAvoidanceTargetObjects( - AvoidancePlanningData & data, DebugData & debug) const + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -223,7 +214,9 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( [&](const auto & object) { return createObjectData(data, object); }); } - utils::avoidance::filterTargetObjects(target_lane_objects, data, debug, planner_data_, p); + utils::avoidance::filterTargetObjects( + target_lane_objects, data, avoidance_parameters_->object_check_max_forward_distance, + planner_data_, p); } ObjectData AvoidanceByLaneChange::createObjectData( @@ -232,6 +225,7 @@ ObjectData AvoidanceByLaneChange::createObjectData( using boost::geometry::return_centroid; using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::calcLateralDeviation; const auto p = std::dynamic_pointer_cast(avoidance_parameters_); @@ -263,12 +257,15 @@ ObjectData AvoidanceByLaneChange::createObjectData( utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, p); // Calc lateral deviation from path to target object. - object_data.lateral = - tier4_autoware_utils::calcLateralDeviation(object_closest_pose, object_pose.position); + object_data.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, data.reference_path, object_data.overhang_pose.position); + object_data.overhang_points = + utils::avoidance::calcEnvelopeOverhangDistance(object_data, data.reference_path); // Check whether the the ego should avoid the object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 3041ae7e1112e..581aafa6c860c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index c68c10f2a9489..4cfc76b6ffae1 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -9,8 +9,6 @@ # avoidance module common setting enable_bound_clipping: false - enable_yield_maneuver: true - enable_yield_maneuver_during_shifting: false enable_cancel_maneuver: true disable_path_update: false @@ -19,6 +17,7 @@ use_opposite_lane: true use_intersection_areas: true use_hatched_road_markings: true + use_freespace_areas: true # for debug publish_debug_marker: false @@ -29,81 +28,97 @@ car: execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] - moving_time_threshold: 1.0 # [s] + moving_time_threshold: 2.0 # [s] + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 # [-] - envelope_buffer_margin: 0.3 # [m] - avoid_margin_lateral: 1.0 # [m] - safety_buffer_lateral: 0.7 # [m] + envelope_buffer_margin: 0.5 # [m] safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h - moving_time_threshold: 1.0 + moving_time_threshold: 2.0 + lateral_margin: + soft_margin: 0.9 # [m] + hard_margin: 0.1 # [m] + hard_margin_for_parked_vehicle: 0.1 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: -0.2 # [m] + hard_margin_for_parked_vehicle: -0.2 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.3 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 0.7 + envelope_buffer_margin: 0.1 safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: execute_num: 1 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.3 # [m] + hard_margin_for_parked_vehicle: 0.3 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 + lateral_margin: + soft_margin: 0.7 # [m] + hard_margin: 0.5 # [m] + hard_margin_for_parked_vehicle: 0.5 # [m] max_expand_ratio: 0.0 - envelope_buffer_margin: 0.8 - avoid_margin_lateral: 1.0 - safety_buffer_lateral: 1.0 + envelope_buffer_margin: 0.5 safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] @@ -122,7 +137,8 @@ motorcycle: true # [-] pedestrian: true # [-] # detection range - object_check_goal_distance: 20.0 # [m] + object_check_goal_distance: 20.0 # [m] + object_check_return_pose_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] object_check_shiftable_ratio: 0.6 # [-] @@ -138,9 +154,12 @@ backward_distance: 10.0 # [m] # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. - force_avoidance: + avoidance_for_ambiguous_vehicle: enable: false # [-] - time_threshold: 1.0 # [s] + closest_distance_to_wait_and_see: 10.0 # [m] + condition: + time_threshold: 1.0 # [s] + distance_threshold: 1.0 # [m] ignore_area: traffic_light: front_distance: 100.0 # [m] @@ -184,6 +203,7 @@ time_horizon_for_rear_object: 10.0 # [s] delay_until_departure: 0.0 # [s] # rss parameters + extended_polygon_policy: "along_path" # [-] select "rectangle" or "along_path" expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] rear_vehicle_reaction_time: 2.0 # [s] @@ -204,6 +224,11 @@ max_right_shift_length: 5.0 max_left_shift_length: 5.0 max_deviation_from_lane: 0.5 # [m] + # approve the next shift line after reaching this percentage of the current shift line length. + # this parameter should be within range of 0.0 to 1.0. + # this parameter is added to allow waiting for the return of shift approval until the occlusion behind the avoid target is clear. + # this feature can be disabled by setting this parameter to 1.0. (in that case, avoidance module creates return shift as soon as possible.) + ratio_for_return_shift_approval: 0.5 # [-] # avoidance distance parameters longitudinal: min_prepare_time: 1.0 # [s] @@ -223,7 +248,8 @@ # For yield maneuver yield: - yield_velocity: 2.78 # [m/s] + enable: true # [-] + enable_during_shifting: false # [-] # For stop maneuver stop: @@ -264,7 +290,8 @@ nominal_jerk: 0.5 # [m/sss] max_deceleration: -1.5 # [m/ss] max_jerk: 1.0 # [m/sss] - max_acceleration: 1.0 # [m/ss] + max_acceleration: 0.5 # [m/ss] + min_velocity_to_limit_max_acceleration: 2.78 # [m/ss] shift_line_pipeline: trim: diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp index 8e294e9e3237e..48b16f1ff2b69 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp @@ -19,12 +19,11 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include #include -#include #include #include @@ -34,6 +33,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -48,10 +48,11 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; -using geometry_msgs::msg::TransformStamped; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; +using route_handler::Direction; + struct ObjectParameter { bool is_avoidance_target{false}; @@ -68,9 +69,11 @@ struct ObjectParameter double envelope_buffer_margin{0.0}; - double avoid_margin_lateral{1.0}; + double lateral_soft_margin{1.0}; + + double lateral_hard_margin{1.0}; - double safety_buffer_lateral{1.0}; + double lateral_hard_margin_for_parked_vehicle{1.0}; double safety_buffer_longitudinal{0.0}; @@ -97,7 +100,7 @@ struct AvoidanceParameters bool enable_cancel_maneuver{false}; // enable avoidance for all parking vehicle - bool enable_force_avoidance_for_stopped_vehicle{false}; + bool enable_avoidance_for_ambiguous_vehicle{false}; // enable yield maneuver. bool enable_yield_maneuver{false}; @@ -114,6 +117,9 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // use freespace area for avoidance + bool use_freespace_areas{false}; + // consider avoidance return dead line bool enable_dead_line_for_goal{false}; bool enable_dead_line_for_traffic_light{false}; @@ -137,6 +143,9 @@ struct AvoidanceParameters // To prevent large acceleration while avoidance. double max_acceleration{0.0}; + // To prevent large acceleration while avoidance. + double min_velocity_to_limit_max_acceleration{0.0}; + // upper distance for envelope polygon expansion. double upper_distance_for_polygon_expansion{0.0}; @@ -163,10 +172,14 @@ struct AvoidanceParameters double object_check_backward_distance{0.0}; double object_check_yaw_deviation{0.0}; - // if the distance between object and goal position is less than this parameter, the module ignore - // the object. + // if the distance between object and goal position is less than this parameter, the module do not + // return center line. double object_check_goal_distance{0.0}; + // if the distance between object and return position is less than this parameter, the module do + // not return center line. + double object_check_return_pose_distance{0.0}; + // use in judge whether the vehicle is parking object on road shoulder double object_check_shiftable_ratio{0.0}; @@ -174,7 +187,9 @@ struct AvoidanceParameters double object_check_min_road_shoulder_width{0.0}; // force avoidance - double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; + double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0}; + double time_threshold_for_ambiguous_vehicle{0.0}; + double distance_threshold_for_ambiguous_vehicle{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction @@ -204,9 +219,6 @@ struct AvoidanceParameters size_t hysteresis_factor_safe_count; double hysteresis_factor_expand_rate{0.0}; - // keep target velocity in yield maneuver - double yield_velocity{0.0}; - // maximum stop distance double stop_max_distance{0.0}; @@ -267,6 +279,9 @@ struct AvoidanceParameters // use for judge if the ego is shifting or not. double lateral_avoid_check_threshold{0.0}; + // use for return shift approval. + double ratio_for_return_shift_approval{0.0}; + // For shift line generation process. The continuous shift length is quantized by this value. double quantize_filter_threshold{0.0}; @@ -326,24 +341,31 @@ struct AvoidanceParameters struct ObjectData // avoidance target { ObjectData() = default; - ObjectData(const PredictedObject & obj, double lat, double lon, double len, double overhang) - : object(obj), lateral(lat), longitudinal(lon), length(len), overhang_dist(overhang) + + ObjectData(PredictedObject obj, double lat, double lon, double len) + : object(std::move(obj)), to_centerline(lat), longitudinal(lon), length(len) { } PredictedObject object; + // object behavior. + enum class Behavior { + NONE = 0, + MERGING, + DEVIATING, + }; + Behavior behavior{Behavior::NONE}; + // lateral position of the CoM, in Frenet coordinate from ego-pose - double lateral; + + double to_centerline{0.0}; // longitudinal position of the CoM, in Frenet coordinate from ego-pose - double longitudinal; + double longitudinal{0.0}; // longitudinal length of vehicle, in Frenet coordinate - double length; - - // lateral distance to the closest footprint, in Frenet coordinate - double overhang_dist; + double length{0.0}; // lateral shiftable ratio double shiftable_ratio{0.0}; @@ -363,11 +385,12 @@ struct ObjectData // avoidance target rclcpp::Time last_move; double stop_time{0.0}; - // store the information of the lanelet which the object's overhang is currently occupying + // It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to + // check whether the object is on the ego lane. lanelet::ConstLanelet overhang_lanelet; - // the position of the overhang - Pose overhang_pose; + // the position at the detected moment + Pose init_pose; // envelope polygon Polygon2d envelope_poly{}; @@ -396,14 +419,29 @@ struct ObjectData // avoidance target // is within intersection area bool is_within_intersection{false}; + // is parked vehicle on road shoulder + bool is_parked{false}; + + // is driving on ego current lane + bool is_on_ego_lane{false}; + + // is ambiguous stopped vehicle. + bool is_ambiguous{false}; + + // object direction. + Direction direction{Direction::NONE}; + + // overhang points (sort by distance) + std::vector> overhang_points{}; + // unavoidable reason - std::string reason{""}; + std::string reason{}; // lateral avoid margin std::optional avoid_margin{std::nullopt}; // the nearest bound point (use in road shoulder distance calculation) - std::optional nearest_bound_point{std::nullopt}; + std::optional> narrowest_place{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -440,14 +478,14 @@ using AvoidLineArray = std::vector; struct AvoidOutline { - AvoidOutline(const AvoidLine & avoid_line, const AvoidLine & return_line) - : avoid_line{avoid_line}, return_line{return_line} + AvoidOutline(AvoidLine avoid_line, const std::optional return_line) + : avoid_line{std::move(avoid_line)}, return_line{std::move(return_line)} { } AvoidLine avoid_line{}; - AvoidLine return_line{}; + std::optional return_line{}; AvoidLineArray middle_lines{}; }; @@ -512,14 +550,16 @@ struct AvoidancePlanningData std::vector drivable_lanes{}; - lanelet::BasicLineString3d right_bound{}; + std::vector right_bound{}; - lanelet::BasicLineString3d left_bound{}; + std::vector left_bound{}; bool safe{false}; bool valid{false}; + bool ready{false}; + bool success{false}; bool comfortable{false}; @@ -566,9 +606,7 @@ struct ShiftLineData */ struct DebugData { - std::shared_ptr current_lanelets; - - geometry_msgs::msg::Polygon detection_area; + std::vector detection_areas; lanelet::ConstLineStrings3d bounds; @@ -618,6 +656,9 @@ struct DebugData // tmp for plot PathWithLaneId center_line; + // safety check area + lanelet::ConstLanelets safety_check_lanes; + // collision check debug map utils::path_safety_checker::CollisionCheckDebugMap collision_check; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp index 2705af5af4020..f49f457ddd066 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp @@ -16,33 +16,23 @@ #define BEHAVIOR_PATH_AVOIDANCE_MODULE__DEBUG_HPP_ #include "behavior_path_avoidance_module/data_structs.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" #include -#include #include -#include #include -#include - #include -#include namespace marker_utils::avoidance_marker { -using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidancePlanningData; -using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; using behavior_path_planner::PathShifter; using behavior_path_planner::ShiftLineArray; -using geometry_msgs::msg::Point; -using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; using visualization_msgs::msg::MarkerArray; @@ -50,7 +40,7 @@ MarkerArray createEgoStatusMarkerArray( const AvoidancePlanningData & data, const Pose & p_ego, std::string && ns); MarkerArray createAvoidLineMarkerArray( - const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g, + const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w); MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::string && ns); @@ -63,11 +53,11 @@ MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); } // namespace marker_utils::avoidance_marker -std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sp_arr); +std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr); std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr); -std::string toStrInfo(const behavior_path_planner::ShiftLine & sp); +std::string toStrInfo(const behavior_path_planner::ShiftLine & sl); std::string toStrInfo(const behavior_path_planner::AvoidLine & ap); diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp index df9640fa75626..4ff02df97bd89 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp @@ -17,12 +17,14 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include #include #include +#include #include namespace behavior_path_planner::helper::avoidance @@ -31,7 +33,10 @@ namespace behavior_path_planner::helper::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::PlannerData; using motion_utils::calcDecelDistWithJerkAndAccConstraints; +using motion_utils::calcLongitudinalOffsetPose; +using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; +using tier4_autoware_utils::getPose; class AvoidanceHelper { @@ -60,7 +65,12 @@ class AvoidanceHelper geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; } - size_t getConstraintsMapIndex(const double velocity, const std::vector & values) const + geometry_msgs::msg::Point getEgoPosition() const + { + return data_->self_odometry->pose.pose.position; + } + + static size_t getConstraintsMapIndex(const double velocity, const std::vector & values) { const auto itr = std::find_if( values.begin(), values.end(), [&](const auto value) { return velocity < value; }); @@ -134,6 +144,24 @@ class AvoidanceHelper shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed()); } + double getFrontConstantDistance(const ObjectData & object) const + { + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front + : 0.0; + return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + } + + double getRearConstantDistance(const ObjectData & object) const + { + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear + + object.length; + } + double getEgoShift() const { validate(); @@ -176,7 +204,8 @@ class AvoidanceHelper { using utils::avoidance::calcShiftLength; - const auto shift_length = calcShiftLength(is_on_right, object.overhang_dist, margin); + const auto shift_length = + calcShiftLength(is_on_right, object.overhang_points.front().first, margin); return is_on_right ? std::min(shift_length, getLeftShiftBound()) : std::max(shift_length, getRightShiftBound()); } @@ -242,6 +271,20 @@ class AvoidanceHelper return *itr; } + std::pair getDistanceToAccelEndPoint(const PathWithLaneId & path) + { + updateAccelEndPoint(path); + + if (!max_v_point_.has_value()) { + return std::make_pair(0.0, std::numeric_limits::max()); + } + + const auto start_idx = data_->findEgoIndex(path.points); + const auto distance = + calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position); + return std::make_pair(distance, max_v_point_.value().second); + } + double getFeasibleDecelDistance( const double target_velocity, const bool use_hard_constraints = true) const { @@ -261,13 +304,70 @@ class AvoidanceHelper bool isComfortable(const AvoidLineArray & shift_lines) const { + const auto JERK_BUFFER = 0.1; // [m/sss] return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { return PathShifter::calcJerkFromLatLonDistance( line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < - getLateralMaxJerkLimit(); + getLateralMaxJerkLimit() + JERK_BUFFER; }); } + bool isReady(const ObjectDataArray & objects) const + { + if (objects.empty()) { + return true; + } + + const auto object = objects.front(); + + if (!object.is_ambiguous) { + return true; + } + + if (!object.avoid_margin.has_value()) { + return true; + } + + const auto is_object_on_right = utils::avoidance::isOnRight(object); + const auto desire_shift_length = + getShiftLength(object, is_object_on_right, object.avoid_margin.value()); + + const auto prepare_distance = getMinimumPrepareDistance(); + const auto constant_distance = getFrontConstantDistance(object); + const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length); + + return object.longitudinal < + prepare_distance + constant_distance + avoidance_distance + + parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle; + } + + bool isReady(const AvoidLineArray & new_shift_lines, const double current_shift_length) const + { + if (std::abs(current_shift_length) < 1e-3) { + return true; + } + + if (new_shift_lines.empty()) { + return true; + } + + const auto front_shift_relative_length = new_shift_lines.front().getRelativeLength(); + + // same direction shift. + if (current_shift_length > 0.0 && front_shift_relative_length > 0.0) { + return true; + } + + // same direction shift. + if (current_shift_length < 0.0 && front_shift_relative_length < 0.0) { + return true; + } + + // keep waiting the other side shift approval until the ego reaches shift length threshold. + const auto ego_shift_ratio = (current_shift_length - getEgoShift()) / current_shift_length; + return ego_shift_ratio < parameters_->ratio_for_return_shift_approval + 1e-3; + } + bool isShifted() const { return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold; @@ -290,6 +390,56 @@ class AvoidanceHelper return true; } + void updateAccelEndPoint(const PathWithLaneId & path) + { + const auto & p = parameters_; + const auto & a_now = data_->self_acceleration->accel.accel.linear.x; + if (a_now < 0.0) { + max_v_point_ = std::nullopt; + return; + } + + if (getEgoSpeed() < p->min_velocity_to_limit_max_acceleration) { + max_v_point_ = std::nullopt; + return; + } + + if (max_v_point_.has_value()) { + return; + } + + const auto v0 = getEgoSpeed() + p->buf_slow_down_speed; + + const auto t_neg_jerk = std::max(0.0, a_now - p->max_acceleration) / p->max_jerk; + const auto v_neg_jerk = v0 + a_now * t_neg_jerk + std::pow(t_neg_jerk, 2.0) / 2.0; + const auto x_neg_jerk = v0 * t_neg_jerk + a_now * std::pow(t_neg_jerk, 2.0) / 2.0 + + p->max_jerk * std::pow(t_neg_jerk, 3.0) / 6.0; + + const auto & v_max = data_->parameters.max_vel; + if (v_max < v_neg_jerk) { + max_v_point_ = std::nullopt; + return; + } + + const auto t_max_accel = (v_max - v_neg_jerk) / p->max_acceleration; + const auto x_max_accel = + v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0; + + const auto point = + calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel); + if (point.has_value()) { + max_v_point_ = std::make_pair(point.value(), v_max); + return; + } + + const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1); + const auto t_end = + (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration; + const auto v_end = v0 + p->max_acceleration * t_end; + + max_v_point_ = std::make_pair(getPose(path.points.back()), v_end); + } + void reset() { prev_reference_path_ = PathWithLaneId(); @@ -340,6 +490,8 @@ class AvoidanceHelper ShiftedPath prev_linear_shift_path_; lanelet::ConstLanelets prev_driving_lanes_; + + std::optional> max_v_point_; }; } // namespace behavior_path_planner::helper::avoidance diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp index 72cfbe6f0a1ed..e23a8a96ee7da 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/manager.hpp @@ -19,11 +19,10 @@ #include "behavior_path_avoidance_module/scene.hpp" #include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include +#include +#include #include -#include -#include #include namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp index 212ed04ade6c6..fbfec58abe4da 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp @@ -41,9 +41,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); - p.enable_yield_maneuver_during_shifting = - getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); p.disable_path_update = getOrDeclareParameter(*node, ns + "disable_path_update"); p.publish_debug_marker = getOrDeclareParameter(*node, ns + "publish_debug_marker"); p.print_debug_info = getOrDeclareParameter(*node, ns + "print_debug_info"); @@ -57,6 +54,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); p.use_hatched_road_markings = getOrDeclareParameter(*node, ns + "use_hatched_road_markings"); + p.use_freespace_areas = getOrDeclareParameter(*node, ns + "use_freespace_areas"); } // target object @@ -71,10 +69,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) param.max_expand_ratio = getOrDeclareParameter(*node, ns + "max_expand_ratio"); param.envelope_buffer_margin = getOrDeclareParameter(*node, ns + "envelope_buffer_margin"); - param.avoid_margin_lateral = - getOrDeclareParameter(*node, ns + "avoid_margin_lateral"); - param.safety_buffer_lateral = - getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); + param.lateral_soft_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.soft_margin"); + param.lateral_hard_margin = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin"); + param.lateral_hard_margin_for_parked_vehicle = + getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); param.safety_buffer_longitudinal = getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); param.use_conservative_buffer_longitudinal = @@ -122,6 +122,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); + p.object_check_return_pose_distance = + getOrDeclareParameter(*node, ns + "object_check_return_pose_distance"); p.threshold_distance_object_is_on_center = getOrDeclareParameter(*node, ns + "threshold_distance_object_is_on_center"); p.object_check_shiftable_ratio = @@ -135,11 +137,14 @@ AvoidanceParameters getParameter(rclcpp::Node * node) } { - const std::string ns = "avoidance.target_filtering.force_avoidance."; - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable"); - p.threshold_time_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "time_threshold"); + const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle."; + p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter(*node, ns + "enable"); + p.closest_distance_to_wait_and_see_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "closest_distance_to_wait_and_see"); + p.time_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.time_threshold"); + p.distance_threshold_for_ambiguous_vehicle = + getOrDeclareParameter(*node, ns + "condition.distance_threshold"); p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); p.object_ignore_section_crosswalk_in_front_distance = @@ -205,6 +210,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) getOrDeclareParameter(*node, ns + "max_velocity"); p.ego_predicted_path_params.acceleration = getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_front_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); p.ego_predicted_path_params.time_resolution = @@ -216,6 +223,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // safety check rss params { const std::string ns = "avoidance.safety_check."; + p.rss_params.extended_polygon_policy = + getOrDeclareParameter(*node, ns + "extended_polygon_policy"); p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = @@ -249,6 +258,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); p.max_deviation_from_lane = getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); + p.ratio_for_return_shift_approval = + getOrDeclareParameter(*node, ns + "ratio_for_return_shift_approval"); + if (p.ratio_for_return_shift_approval < 0.0 || p.ratio_for_return_shift_approval > 1.0) { + throw std::domain_error( + "ratio_for_return_shift_approval should be within range of 0.0 to 1.0"); + } } // avoidance maneuver (longitudinal) @@ -277,7 +292,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node) // yield { const std::string ns = "avoidance.yield."; - p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); + p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable"); + p.enable_yield_maneuver_during_shifting = + getOrDeclareParameter(*node, ns + "enable_during_shifting"); } // stop @@ -296,6 +313,10 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.use_shorten_margin_immediately = getOrDeclareParameter(*node, ns + "use_shorten_margin_immediately"); + if (p.policy_approval != "per_shift_line" && p.policy_approval != "per_avoidance_maneuver") { + throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); + } + if (p.policy_deceleration != "best_effort" && p.policy_deceleration != "reliable") { throw std::domain_error("invalid policy. please select 'best_effort' or 'reliable'."); } @@ -313,6 +334,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); p.max_jerk = getOrDeclareParameter(*node, ns + "max_jerk"); p.max_acceleration = getOrDeclareParameter(*node, ns + "max_acceleration"); + p.min_velocity_to_limit_max_acceleration = + getOrDeclareParameter(*node, ns + "min_velocity_to_limit_max_acceleration"); } // constraints (lateral) diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp index 882806177b4e0..77e2a801edc72 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp @@ -20,9 +20,10 @@ #include "behavior_path_avoidance_module/shift_line_generator.hpp" #include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include +#include +#include +#include #include #include @@ -30,11 +31,9 @@ #include #include -#include #include #include #include -#include #include namespace behavior_path_planner @@ -113,34 +112,22 @@ class AvoidanceModule : public SceneModuleInterface */ void updateRegisteredRTCStatus(const PathWithLaneId & path) { - const Point ego_position = planner_data_->self_odometry->pose.pose.position; + const auto ego_idx = planner_data_->findEgoIndex(path.points); for (const auto & left_shift : left_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, left_shift.start_pose.position); + const double finish_distance = start_distance + left_shift.relative_longitudinal; rtc_interface_ptr_map_.at("left")->updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); - if (finish_distance > -1.0e-03) { - steering_factor_interface_ptr_->updateSteeringFactor( - {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, SteeringFactor::LEFT, SteeringFactor::TURNING, ""); - } } for (const auto & right_shift : right_shift_array_) { const double start_distance = - calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); - const double finish_distance = - calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); + calcSignedArcLength(path.points, ego_idx, right_shift.start_pose.position); + const double finish_distance = start_distance + right_shift.relative_longitudinal; rtc_interface_ptr_map_.at("right")->updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); - if (finish_distance > -1.0e-03) { - steering_factor_interface_ptr_->updateSteeringFactor( - {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - PlanningBehavior::AVOIDANCE, SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); - } } } @@ -239,11 +226,10 @@ class AvoidanceModule : public SceneModuleInterface void insertPrepareVelocity(ShiftedPath & shifted_path) const; /** - * @brief insert decel point in output path in order to yield. the ego decelerates within - * accel/jerk constraints. + * @brief insert max velocity in output path to limit acceleration. * @param target path. */ - void insertYieldVelocity(ShiftedPath & shifted_path) const; + void insertAvoidanceVelocity(ShiftedPath & shifted_path) const; /** * @brief calculate stop distance based on object's overhang. @@ -326,13 +312,6 @@ class AvoidanceModule : public SceneModuleInterface // generate output data - /** - * @brief calculate turn signal infomation. - * @param avoidance path. - * @return turn signal command. - */ - TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - /** * @brief fill debug markers. */ @@ -374,8 +353,8 @@ class AvoidanceModule : public SceneModuleInterface */ void removeRegisteredShiftLines() { - constexpr double THRESHOLD = 0.1; - if (std::abs(path_shifter_.getBaseOffset()) > THRESHOLD) { + constexpr double threshold = 0.1; + if (std::abs(path_shifter_.getBaseOffset()) > threshold) { RCLCPP_INFO(getLogger(), "base offset is not zero. can't reset registered shift lines."); return; } @@ -396,7 +375,7 @@ class AvoidanceModule : public SceneModuleInterface * @brief remove behind shift lines. * @param path shifter. */ - void postProcess() + void postProcess() override { const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); @@ -407,16 +386,19 @@ class AvoidanceModule : public SceneModuleInterface UUID uuid; Pose start_pose; Pose finish_pose; + double relative_longitudinal{0.0}; }; using RegisteredShiftLineArray = std::vector; - bool is_avoidance_maneuver_starts; - bool arrived_path_end_{false}; bool safe_{true}; + bool is_recording_{false}; + + bool is_record_necessary_{false}; + std::shared_ptr helper_; std::shared_ptr parameters_; @@ -433,14 +415,20 @@ class AvoidanceModule : public SceneModuleInterface UUID candidate_uuid_; + // TODO(Satoshi OTA) create detected object manager. ObjectDataArray registered_objects_; - mutable size_t safe_count_{0}; + // TODO(Satoshi OTA) remove mutable. + mutable ObjectDataArray detected_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray ego_stopped_objects_; + // TODO(Satoshi OTA) remove this variable. mutable ObjectDataArray stopped_objects_; + mutable size_t safe_count_{0}; + mutable DebugData debug_data_; mutable std::shared_ptr debug_msg_ptr_; diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp index edfe9ab9663ce..efdda622a664c 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/shift_line_generator.hpp @@ -17,11 +17,8 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/helper.hpp" -#include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include - #include namespace behavior_path_planner::utils::avoidance @@ -29,8 +26,6 @@ namespace behavior_path_planner::utils::avoidance using behavior_path_planner::PathShifter; using behavior_path_planner::helper::avoidance::AvoidanceHelper; -using motion_utils::calcSignedArcLength; -using motion_utils::findNearestIndex; class ShiftLineGenerator { diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp index 521058e3a43c9..3f1d3495e51cb 100644 --- a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp +++ b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp @@ -36,6 +36,9 @@ bool isOnRight(const ObjectData & obj); double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); +bool isWithinLanes( + const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data); + bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length); bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length); @@ -56,8 +59,8 @@ double lerpShiftLengthOnArc(double arc, const AvoidLine & al); void fillLongitudinalAndLengthByClosestEnvelopeFootprint( const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj); -double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose); +std::vector> calcEnvelopeOverhangDistance( + const ObjectData & object_data, const PathWithLaneId & path); void setEndData( AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx, @@ -77,19 +80,11 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); - -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets); - lanelet::ConstLanelets getAdjacentLane( + const lanelet::ConstLanelet & current_lane, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset); - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); @@ -117,6 +112,8 @@ void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters); +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects); + void updateRegisteredObject( ObjectDataArray & registered_objects, const ObjectDataArray & now_objects, const std::shared_ptr & parameters); @@ -126,13 +123,12 @@ void compensateDetectionLost( ObjectDataArray & other_objects); void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); @@ -151,14 +147,18 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift); + const std::shared_ptr & parameters, const bool has_left_shift, + const bool has_right_shift, DebugData & debug); std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug); + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug); -DrivableLanes generateExpandDrivableLanes( +DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet); + +DrivableLanes generateExpandedDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); @@ -171,6 +171,7 @@ double calcDistanceToAvoidStartLine( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); + } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_avoidance_module/package.xml b/planning/behavior_path_avoidance_module/package.xml index 642f83497acfb..fc34172bb2ce9 100644 --- a/planning/behavior_path_avoidance_module/package.xml +++ b/planning/behavior_path_avoidance_module/package.xml @@ -6,6 +6,7 @@ The behavior_path_avoidance_module package Takamasa Horibe + Zulfaqar Azmi Satoshi Ota Fumiya Watanabe Kyoichi Sugahara @@ -27,7 +28,6 @@ behavior_path_planner_common geometry_msgs lanelet2_extension - libboost-dev magic_enum motion_utils objects_of_interest_marker_interface diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp index 310613ee19678..7d029277fcbc4 100644 --- a/planning/behavior_path_avoidance_module/src/debug.cpp +++ b/planning/behavior_path_avoidance_module/src/debug.cpp @@ -14,16 +14,15 @@ #include "behavior_path_avoidance_module/debug.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include #include #include #include -#include -#include - #include #include @@ -48,7 +47,7 @@ int32_t uuidToInt32(const unique_identifier_msgs::msg::UUID & uuid) int32_t ret = 0; for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); + ret <<= sizeof(int8_t) * 8; ret |= uuid.uuid.at(i); } @@ -114,7 +113,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: MarkerArray msg; for (const auto & object : objects) { - if (!object.nearest_bound_point.has_value()) { + if (!object.narrowest_place.has_value()) { continue; } @@ -123,8 +122,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 0.0, 0.42, 0.999)); - marker.points.push_back(object.overhang_pose.position); - marker.points.push_back(object.nearest_bound_point.value()); + marker.points.push_back(object.narrowest_place.value().first); + marker.points.push_back(object.narrowest_place.value().second); marker.id = uuidToInt32(object.object.object_id); msg.markers.push_back(marker); } @@ -134,7 +133,7 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std:: "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); - marker.pose.position = object.nearest_bound_point.value(); + marker.pose.position = object.narrowest_place.value().second; std::ostringstream string_stream; string_stream << object.to_road_shoulder_distance << "[m]"; marker.text = string_stream.str(); @@ -160,7 +159,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st std::ostringstream string_stream; string_stream << std::fixed << std::setprecision(2) << std::boolalpha; string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" - << "lateral:" << object.lateral << " [m]\n" + << "lateral:" << object.to_centerline << " [m]\n" << "necessity:" << object.avoid_required << " [-]\n" << "stoppable:" << object.is_stoppable << " [-]\n" << "stop_factor:" << object.to_stop_factor_distance << " [m]\n" @@ -177,7 +176,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st marker.id = uuidToInt32(object.object.object_id); marker.pose.position.z += 2.0; std::ostringstream string_stream; - string_stream << object.reason; + string_stream << object.reason << (object.is_parked ? "(PARKED)" : ""); marker.text = string_stream.str(); marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999); marker.scale = createMarkerScale(0.6, 0.6, 0.6); @@ -189,10 +188,25 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st return msg; } +MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + msg.markers.reserve(objects.size()); + + for (const auto & object : objects) { + appendMarkerArray( + marker_utils::createLaneletsAreaMarkerArray( + {object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0), + &msg); + } + + return msg; +} + MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - msg.markers.reserve(objects.size() * 4); + msg.markers.reserve(objects.size() * 5); appendMarkerArray( createObjectsCubeMarkerArray( @@ -203,6 +217,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -210,7 +225,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns) { MarkerArray msg; - msg.markers.reserve(objects.size() * 4); + msg.markers.reserve(objects.size() * 5); appendMarkerArray( createObjectsCubeMarkerArray( @@ -221,6 +236,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std:: appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg); appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg); + appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg); return msg; } @@ -452,6 +468,10 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const appendMarkerArray( createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"), &msg); + appendMarkerArray( + createOverhangLaneletMarkerArray( + filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"), + &msg); return msg; } @@ -470,7 +490,7 @@ MarkerArray createDrivableBounds( createMarkerColor(r, g, b, 0.999)); for (const auto & p : data.right_bound) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(p); } msg.markers.push_back(marker); @@ -483,7 +503,7 @@ MarkerArray createDrivableBounds( createMarkerColor(r, g, b, 0.999)); for (const auto & p : data.left_bound) { - marker.points.push_back(createPoint(p.x(), p.y(), p.z())); + marker.points.push_back(p); } msg.markers.push_back(marker); @@ -495,6 +515,8 @@ MarkerArray createDrivableBounds( MarkerArray createDebugMarkerArray( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) { + using behavior_path_planner::utils::transformToLanelets; + using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::createLaneletsAreaMarkerArray; using marker_utils::createObjectsMarkerArray; using marker_utils::createPathMarkerArray; @@ -554,6 +576,11 @@ MarkerArray createDebugMarkerArray( addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); addObjects(data.other_objects, std::string("TooNearToGoal")); + addObjects(data.other_objects, std::string("ParallelToEgoLane")); + addObjects(data.other_objects, std::string("MergingToEgoLane")); + addObjects(data.other_objects, std::string("UnstableObject")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle")); + addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)")); } // shift line pre-process @@ -605,12 +632,23 @@ MarkerArray createDebugMarkerArray( addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } + // detection area + size_t i = 0; + for (const auto & detection_area : debug.detection_areas) { + add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1)); + } + // misc { add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42)); + add(laneletsAsTriangleMarkerArray( + "drivable_lanes", transformToLanelets(data.drivable_lanes), + createMarkerColor(0.16, 1.0, 0.69, 0.2))); + add(laneletsAsTriangleMarkerArray( + "current_lanes", data.current_lanelets, createMarkerColor(1.0, 1.0, 1.0, 0.2))); + add(laneletsAsTriangleMarkerArray( + "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2))); } return msg; diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp index 68cee1aa2b523..5ce63987621ed 100644 --- a/planning/behavior_path_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_module/src/manager.cpp @@ -60,8 +60,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); updateParam(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin); - updateParam(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral); - updateParam(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); + updateParam(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin); + updateParam(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin); + updateParam( + parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", + config.lateral_hard_margin_for_parked_vehicle); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); updateParam( @@ -88,6 +91,76 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorupper_distance_for_polygon_expansion); } + { + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p->object_parameters.count(object_type) == 0) { + return; + } + updateParam(parameters, ns, p->object_parameters.at(object_type).is_avoidance_target); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + + updateParam( + parameters, ns + "object_check_goal_distance", p->object_check_goal_distance); + updateParam( + parameters, ns + "object_check_return_pose_distance", p->object_check_return_pose_distance); + updateParam( + parameters, ns + "threshold_distance_object_is_on_center", + p->threshold_distance_object_is_on_center); + updateParam( + parameters, ns + "object_check_shiftable_ratio", p->object_check_shiftable_ratio); + updateParam( + parameters, ns + "object_check_min_road_shoulder_width", + p->object_check_min_road_shoulder_width); + updateParam( + parameters, ns + "object_last_seen_threshold", p->object_last_seen_threshold); + } + + { + const std::string ns = "avoidance.target_filtering.detection_area."; + updateParam(parameters, ns + "static", p->use_static_detection_area); + updateParam( + parameters, ns + "min_forward_distance", p->object_check_min_forward_distance); + updateParam( + parameters, ns + "max_forward_distance", p->object_check_max_forward_distance); + updateParam(parameters, ns + "backward_distance", p->object_check_backward_distance); + } + + { + const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle."; + updateParam(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle); + updateParam( + parameters, ns + "closest_distance_to_wait_and_see", + p->closest_distance_to_wait_and_see_for_ambiguous_vehicle); + updateParam( + parameters, ns + "condition.time_threshold", p->time_threshold_for_ambiguous_vehicle); + updateParam( + parameters, ns + "condition.distance_threshold", p->distance_threshold_for_ambiguous_vehicle); + updateParam( + parameters, ns + "ignore_area.traffic_light.front_distance", + p->object_ignore_section_traffic_light_in_front_distance); + updateParam( + parameters, ns + "ignore_area.crosswalk.front_distance", + p->object_ignore_section_crosswalk_in_front_distance); + updateParam( + parameters, ns + "ignore_area.crosswalk.behind_distance", + p->object_ignore_section_crosswalk_behind_distance); + } + + { + const std::string ns = "avoidance.target_filtering.intersection."; + updateParam(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation); + } + { const std::string ns = "avoidance.avoidance.lateral."; updateParam( @@ -104,6 +177,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "min_prepare_time", p->min_prepare_time); updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); + updateParam(parameters, ns + "min_prepare_distance", p->min_prepare_distance); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); } @@ -114,6 +188,33 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_buffer", p->stop_buffer); } + { + const std::string ns = "avoidance.policy."; + updateParam(parameters, ns + "make_approval_request", p->policy_approval); + updateParam(parameters, ns + "deceleration", p->policy_deceleration); + updateParam(parameters, ns + "lateral_margin", p->policy_lateral_margin); + updateParam( + parameters, ns + "use_shorten_margin_immediately", p->use_shorten_margin_immediately); + + if (p->policy_approval != "per_shift_line" && p->policy_approval != "per_avoidance_maneuver") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid policy. please select 'per_shift_line' or 'per_avoidance_maneuver'."); + } + + if (p->policy_deceleration != "best_effort" && p->policy_deceleration != "reliable") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid deceleration policy. Please select 'best_effort' or 'reliable'."); + } + + if (p->policy_lateral_margin != "best_effort" && p->policy_lateral_margin != "reliable") { + RCLCPP_ERROR( + rclcpp::get_logger(__func__), + "invalid lateral margin policy. Please select 'best_effort' or 'reliable'."); + } + } + { const std::string ns = "avoidance.constrains.lateral."; @@ -143,6 +244,19 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "nominal_deceleration", p->nominal_deceleration); + updateParam(parameters, ns + "nominal_jerk", p->nominal_jerk); + updateParam(parameters, ns + "max_deceleration", p->max_deceleration); + updateParam(parameters, ns + "max_jerk", p->max_jerk); + updateParam(parameters, ns + "max_acceleration", p->max_acceleration); + updateParam( + parameters, ns + "min_velocity_to_limit_max_acceleration", + p->min_velocity_to_limit_max_acceleration); + } + { const std::string ns = "avoidance.shift_line_pipeline."; updateParam( diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index ebe42f73d538b..b7a732359e601 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -17,10 +17,11 @@ #include "behavior_path_avoidance_module/debug.hpp" #include "behavior_path_avoidance_module/utils.hpp" #include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" #include @@ -28,14 +29,9 @@ #include #include #include -#include -#include -#include -#include - -#include -#include +#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include #include #include @@ -97,6 +93,34 @@ lanelet::BasicLineString3d toLineString3d(const std::vector & bound) bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); return ret; } + +bool straddleRoadBound( + const ShiftedPath & path, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (const auto & p : path.path.points) { + const auto transform = pose2transform(p.point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -137,7 +161,7 @@ bool AvoidanceModule::isExecutionRequested() const bool AvoidanceModule::isExecutionReady() const { DEBUG_PRINT("AVOIDANCE isExecutionReady"); - return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid; + return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready; } bool AvoidanceModule::isSatisfiedSuccessCondition(const AvoidancePlanningData & data) const @@ -226,21 +250,52 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); // expand drivable lanes + const auto is_within_current_lane = + utils::avoidance::isWithinLanes(data.current_lanelets, planner_data_); + const auto red_signal_lane_itr = std::find_if( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + const auto next_lanes = planner_data_->route_handler->getNextLanelets(lanelet); + return utils::traffic_light::isTrafficSignalStop(next_lanes, planner_data_); + }); + const auto not_use_adjacent_lane = + is_within_current_lane && red_signal_lane_itr != data.current_lanelets.end(); + std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - data.drivable_lanes.push_back( - utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + if (!not_use_adjacent_lane) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + } else if (red_signal_lane_itr->id() != lanelet.id()) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + } else { + data.drivable_lanes.push_back(utils::avoidance::generateNotExpandedDrivableLanes(lanelet)); + } }); // calc drivable bound auto tmp_path = getPreviousModuleOutput().path; const auto shorten_lanes = utils::cutOverlappedLanes(tmp_path, data.drivable_lanes); - data.left_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, true)); - data.right_bound = toLineString3d(utils::calcBound( - planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, - parameters_->use_intersection_areas, false)); + const auto use_left_side_hatched_road_marking_area = [&]() { + if (!not_use_adjacent_lane) { + return true; + } + return !planner_data_->route_handler->getRoutingGraphPtr()->left(*red_signal_lane_itr); + }(); + const auto use_right_side_hatched_road_marking_area = [&]() { + if (!not_use_adjacent_lane) { + return true; + } + return !planner_data_->route_handler->getRoutingGraphPtr()->right(*red_signal_lane_itr); + }(); + data.left_bound = utils::calcBound( + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + use_left_side_hatched_road_marking_area, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, true); + data.right_bound = utils::calcBound( + getPreviousModuleOutput().path, planner_data_, shorten_lanes, + use_right_side_hatched_road_marking_area, parameters_->use_intersection_areas, + parameters_->use_freespace_areas, false); // reference path if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { @@ -288,25 +343,21 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + using utils::avoidance::fillAvoidanceNecessity; using utils::avoidance::fillObjectStoppableJudge; using utils::avoidance::filterTargetObjects; - using utils::avoidance::getTargetLanelets; - - // Add margin in order to prevent avoidance request chattering only when the module is running. - const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || - getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + using utils::avoidance::separateObjectsByPath; + using utils::avoidance::updateRoadShoulderDistance; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. - const auto sparse_resample_path = utils::resamplePathWithSpline( - helper_->getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); - const auto [object_within_target_lane, object_outside_target_lane] = - utils::avoidance::separateObjectsByPath( - sparse_resample_path, planner_data_, data, parameters_, helper_->getForwardDetectionRange(), - is_running, debug); + constexpr double MARGIN = 10.0; + const auto forward_detection_range = helper_->getForwardDetectionRange(); + const auto [object_within_target_lane, object_outside_target_lane] = separateObjectsByPath( + helper_->getPreviousReferencePath(), helper_->getPreviousSplineShiftPath().path, planner_data_, + data, parameters_, forward_detection_range + MARGIN, debug); for (const auto & object : object_outside_target_lane.objects) { - ObjectData other_object; - other_object.object = object; + ObjectData other_object = createObjectData(data, object); other_object.reason = "OutOfTargetArea"; data.other_objects.push_back(other_object); } @@ -317,25 +368,26 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } // Filter out the objects to determine the ones to be avoided. - filterTargetObjects(objects, data, debug, planner_data_, parameters_); + filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_); + updateRoadShoulderDistance(data, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. + const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { + fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_); o.to_stop_line = calcDistanceToStopLine(o); fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); }); // debug { - debug.current_lanelets = std::make_shared(data.current_lanelets); - std::vector debug_info_array; const auto append = [&](const auto & o) { AvoidanceDebugMsg debug_info; debug_info.object_id = toHexString(o.object.object_id); debug_info.longitudinal_distance = o.longitudinal; - debug_info.lateral_distance_from_centerline = o.lateral; + debug_info.lateral_distance_from_centerline = o.to_centerline; debug_info.allow_avoidance = o.reason == ""; debug_info.failed_reason = o.reason; debug_info_array.push_back(debug_info); @@ -379,17 +431,13 @@ ObjectData AvoidanceModule::createObjectData( // Calc moving time. utils::avoidance::fillObjectMovingTime(object_data, stopped_objects_, parameters_); - // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); - - // Find the footprint point closest to the path, set to object_data.overhang_distance. - object_data.overhang_dist = utils::avoidance::calcEnvelopeOverhangDistance( - object_data, data.reference_path, object_data.overhang_pose.position); + // Fill init pose. + utils::avoidance::fillInitialPose(object_data, detected_objects_); - // Check whether the the ego should avoid the object. - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - utils::avoidance::fillAvoidanceNecessity( - object_data, registered_objects_, vehicle_width, parameters_); + // Calc lateral deviation from path to target object. + object_data.direction = calcLateralDeviation(object_closest_pose, object_pose.position) > 0.0 + ? Direction::LEFT + : Direction::RIGHT; return object_data; } @@ -492,6 +540,8 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de */ data.comfortable = helper_->isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); + data.ready = helper_->isReady(data.new_shift_line, path_shifter_.getLastShiftLength()) && + helper_->isReady(data.target_objects); } void AvoidanceModule::fillEgoStatus( @@ -607,21 +657,19 @@ void AvoidanceModule::fillDebugData( const auto o_front = data.stop_target_object.value(); const auto object_type = utils::getHighestProbLabel(o_front.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; + const auto constant_distance = helper_->getFrontConstantDistance(o_front); const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto lateral_hard_margin = o_front.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto max_avoid_margin = lateral_hard_margin * o_front.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; - const auto variable = helper_->getSharpAvoidanceDistance( + const auto avoidance_distance = helper_->getSharpAvoidanceDistance( helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); - const auto constant = helper_->getNominalPrepareDistance() + - object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal; - const auto total_avoid_distance = variable + constant; + const auto prepare_distance = helper_->getNominalPrepareDistance(); + const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance; dead_pose_ = calcLongitudinalOffsetPose( data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); @@ -659,13 +707,13 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif } insertPrepareVelocity(path); + insertAvoidanceVelocity(path); switch (data.state) { case AvoidanceState::NOT_AVOID: { break; } case AvoidanceState::YIELD: { - insertYieldVelocity(path); insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); break; } @@ -699,43 +747,56 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const bool limit_to_max_velocity = false; - const auto ego_predicted_path_params = - std::make_shared( - parameters_->ego_predicted_path_params); - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); - const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - true, limit_to_max_velocity); - const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, - false, limit_to_max_velocity); - const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); - const auto is_right_shift = [&]() -> std::optional { + + const auto has_left_shift = [&]() { for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { const auto length = shifted_path.shift_length.at(i); if (parameters_->lateral_avoid_check_threshold < length) { - return false; + return true; } + } + + return false; + }(); + + const auto has_right_shift = [&]() { + for (size_t i = ego_idx; i < shifted_path.shift_length.size(); i++) { + const auto length = shifted_path.shift_length.at(i); if (parameters_->lateral_avoid_check_threshold < -1.0 * length) { return true; } } - return std::nullopt; + return false; }(); - if (!is_right_shift.has_value()) { + if (!has_left_shift && !has_right_shift) { return true; } const auto hysteresis_factor = safe_ ? 1.0 : parameters_->hysteresis_factor_expand_rate; const auto safety_check_target_objects = utils::avoidance::getSafetyCheckTargetObjects( - avoid_data_, planner_data_, parameters_, is_right_shift.value()); + avoid_data_, planner_data_, parameters_, has_left_shift, has_right_shift, debug); + + if (safety_check_target_objects.empty()) { + return true; + } + + const bool limit_to_max_velocity = false; + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); for (const auto & object : safety_check_target_objects) { auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); @@ -789,15 +850,16 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; - for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); - } - for (const auto & sp : generator_.getRawRegisteredShiftLine()) { - max_dist = std::max( - max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); + auto lines = path_shifter_.getShiftLines(); + if (lines.empty()) { + return max_dist; } - return max_dist; + std::sort(lines.begin(), lines.end(), [](const auto & a, const auto & b) { + return a.start_idx < b.start_idx; + }); + return std::max( + max_dist, + calcSignedArcLength(previous_path.points, lines.front().start.position, getEgoPosition())); }(); const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. @@ -871,7 +933,7 @@ BehaviorModuleOutput AvoidanceModule::plan() if (success_spline_path_generation && success_linear_path_generation) { helper_->setPreviousLinearShiftPath(linear_shift_path); helper_->setPreviousSplineShiftPath(spline_shift_path); - helper_->setPreviousReferencePath(data.reference_path); + helper_->setPreviousReferencePath(path_shifter_.getReferencePath()); } else { spline_shift_path = helper_->getPreviousSplineShiftPath(); } @@ -884,9 +946,17 @@ BehaviorModuleOutput AvoidanceModule::plan() BehaviorModuleOutput output; // turn signal info - { + if (path_shifter_.getShiftLines().empty()) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = calcTurnSignalInfo(spline_shift_path); + + constexpr bool is_driving_forward = true; + constexpr bool egos_lane_is_shifted = true; + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + linear_shift_path, path_shifter_.getShiftLines().front(), avoid_data_.current_lanelets, + helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, @@ -894,6 +964,48 @@ BehaviorModuleOutput AvoidanceModule::plan() planner_data_->parameters.ego_nearest_yaw_threshold); } + // check if the ego straddles lane border + if (!is_recording_) { + is_record_necessary_ = straddleRoadBound( + spline_shift_path, data.current_lanelets, planner_data_->parameters.vehicle_info); + } + + if (!path_shifter_.getShiftLines().empty()) { + const auto front_shift_line = path_shifter_.getShiftLines().front(); + const double start_distance = calcSignedArcLength( + data.reference_path.points, getEgoPosition(), front_shift_line.start.position); + const double finish_distance = calcSignedArcLength( + data.reference_path.points, getEgoPosition(), front_shift_line.end.position); + + const auto record_start_time = !is_recording_ && is_record_necessary_ && helper_->isShifted(); + const auto record_end_time = is_recording_ && !helper_->isShifted(); + const auto relative_shift_length = front_shift_line.end_shift_length - helper_->getEgoShift(); + const auto steering_direction = + relative_shift_length > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + const auto steering_status = [&]() { + if (record_start_time) { + is_recording_ = true; + RCLCPP_ERROR(getLogger(), "start right avoidance maneuver. get time stamp."); + return uint16_t(100); + } + + if (record_end_time) { + is_recording_ = false; + is_record_necessary_ = false; + RCLCPP_ERROR(getLogger(), "end avoidance maneuver. get time stamp."); + return uint16_t(200); + } + + return helper_->isShifted() ? SteeringFactor::TURNING : SteeringFactor::APPROACHING; + }(); + + if (finish_distance > -1.0e-03) { + steering_factor_interface_ptr_->updateSteeringFactor( + {front_shift_line.start, front_shift_line.end}, {start_distance, finish_distance}, + PlanningBehavior::AVOIDANCE, steering_direction, steering_status, ""); + } + } + // sparse resampling for computational cost { spline_shift_path.path = utils::resamplePathWithSpline( @@ -926,17 +1038,34 @@ BehaviorModuleOutput AvoidanceModule::plan() { DrivableAreaInfo current_drivable_area_info; // generate drivable lanes - current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; - // generate obstacle polygons - current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + current_drivable_area_info.drivable_lanes.push_back( + utils::avoidance::generateExpandedDrivableLanes(lanelet, planner_data_, parameters_)); + }); // expand hatched road markings current_drivable_area_info.enable_expanding_hatched_road_markings = parameters_->use_hatched_road_markings; // expand intersection areas current_drivable_area_info.enable_expanding_intersection_areas = parameters_->use_intersection_areas; + // expand freespace areas + current_drivable_area_info.enable_expanding_freespace_areas = parameters_->use_freespace_areas; + // generate obstacle polygons + if (parameters_->enable_bound_clipping) { + ObjectDataArray clip_objects; + // If avoidance is executed by both behavior and motion, only non-avoidable object will be + // extracted from the drivable area. + std::for_each( + data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { + if (!object.is_avoidable) clip_objects.push_back(object); + }); + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + clip_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + } else { + current_drivable_area_info.obstacles.clear(); + } output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); @@ -974,14 +1103,6 @@ CandidateOutput AvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; - }); - steering_factor_interface_ptr_->updateSteeringFactor( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - PlanningBehavior::AVOIDANCE, steering_factor_direction, SteeringFactor::APPROACHING, ""); - output.path_candidate = shifted_path.path; return output; } @@ -1021,11 +1142,14 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) const auto sl = helper_->getMainShiftLine(shift_lines); const auto sl_front = shift_lines.front(); const auto sl_back = shift_lines.back(); + const auto relative_longitudinal = sl_back.end_longitudinal - sl_front.start_longitudinal; if (helper_->getRelativeShiftToPath(sl) > 0.0) { - left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); + left_shift_array_.push_back( + {uuid_map_.at("left"), sl_front.start, sl_back.end, relative_longitudinal}); } else if (helper_->getRelativeShiftToPath(sl) < 0.0) { - right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); + right_shift_array_.push_back( + {uuid_map_.at("right"), sl_front.start, sl_back.end, relative_longitudinal}); } uuid_map_.at("left") = generateUUID(); @@ -1133,28 +1257,45 @@ bool AvoidanceModule::isValidShiftLine( } } + const auto is_return_shift = + [](const double start_shift_length, const double end_shift_length, const double threshold) { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + // check if the vehicle is in road. (yaw angle is not considered) { const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + parameters_->hard_road_shoulder_margin - parameters_->max_deviation_from_lane; - const size_t start_idx = shift_lines.front().start_idx; - const size_t end_idx = shift_lines.back().end_idx; + for (const auto & shift_line : shift_lines) { + const size_t start_idx = shift_line.start_idx; + const size_t end_idx = shift_line.end_idx; - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); - lanelet::BasicPoint2d basic_point{p.x, p.y}; + if (is_return_shift( + shift_line.start_shift_length, shift_line.end_shift_length, + parameters_->lateral_small_shift_threshold)) { + continue; + } - const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; - const auto THRESHOLD = minimum_distance + std::abs(shift_length); + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); + const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(path.points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; - if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { - RCLCPP_DEBUG_THROTTLE( - getLogger(), *clock_, 1000, - "following latest new shift line may cause deviation from drivable area."); - return false; + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } } } } @@ -1205,6 +1346,12 @@ void AvoidanceModule::updateData() // update rtc status. updateRTCData(); + // update interest objects data + for (const auto & [uuid, data] : debug_data_.collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + safe_ = avoid_data_.safe; } @@ -1229,7 +1376,6 @@ void AvoidanceModule::initVariables() debug_marker_.markers.clear(); resetPathCandidate(); resetPathReference(); - is_avoidance_maneuver_starts = false; arrived_path_end_ = false; } @@ -1266,8 +1412,8 @@ void AvoidanceModule::updateRTCData() CandidateOutput output; - const auto sl_front = candidates.front(); - const auto sl_back = candidates.back(); + const auto & sl_front = candidates.front(); + const auto & sl_back = candidates.back(); output.path_candidate = data.candidate_path.path; output.lateral_shift = helper_->getRelativeShiftToPath(shift_line); @@ -1277,118 +1423,6 @@ void AvoidanceModule::updateRTCData() updateCandidateRTCStatus(output); } -TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) const -{ - const auto shift_lines = path_shifter_.getShiftLines(); - if (shift_lines.empty()) { - return {}; - } - - const auto front_shift_line = shift_lines.front(); - const size_t start_idx = front_shift_line.start_idx; - const size_t end_idx = front_shift_line.end_idx; - - const auto current_shift_length = helper_->getEgoShift(); - const double start_shift_length = path.shift_length.at(start_idx); - const double end_shift_length = path.shift_length.at(end_idx); - const double segment_shift_length = end_shift_length - start_shift_length; - - const double turn_signal_shift_length_threshold = - planner_data_->parameters.turn_signal_shift_length_threshold; - const double turn_signal_search_time = planner_data_->parameters.turn_signal_search_time; - const double turn_signal_minimum_search_distance = - planner_data_->parameters.turn_signal_minimum_search_distance; - - // If shift length is shorter than the threshold, it does not need to turn on blinkers - if (std::fabs(segment_shift_length) < turn_signal_shift_length_threshold) { - return {}; - } - - // If the vehicle does not shift anymore, we turn off the blinker - if (std::fabs(end_shift_length - current_shift_length) < 0.1) { - return {}; - } - - // compute blinker start idx and end idx - size_t blinker_start_idx = [&]() { - for (size_t idx = start_idx; idx <= end_idx; ++idx) { - const double current_shift_length = path.shift_length.at(idx); - if (current_shift_length > 0.1) { - return idx; - } - } - return start_idx; - }(); - size_t blinker_end_idx = end_idx; - - // prevent invalid access for out-of-range - blinker_start_idx = - std::min(std::max(std::size_t(0), blinker_start_idx), path.path.points.size() - 1); - blinker_end_idx = - std::min(std::max(blinker_start_idx, blinker_end_idx), path.path.points.size() - 1); - - const auto blinker_start_pose = path.path.points.at(blinker_start_idx).point.pose; - const auto blinker_end_pose = path.path.points.at(blinker_end_idx).point.pose; - - const double ego_vehicle_offset = - planner_data_->parameters.vehicle_info.max_longitudinal_offset_m; - const auto signal_prepare_distance = - std::max(getEgoSpeed() * turn_signal_search_time, turn_signal_minimum_search_distance); - const auto ego_front_to_shift_start = - calcSignedArcLength(path.path.points, getEgoPosition(), blinker_start_pose.position) - - ego_vehicle_offset; - - if (signal_prepare_distance < ego_front_to_shift_start) { - return {}; - } - - bool turn_signal_on_swerving = planner_data_->parameters.turn_signal_on_swerving; - - TurnSignalInfo turn_signal_info{}; - if (turn_signal_on_swerving) { - if (segment_shift_length > 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } else { - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); - const auto local_vehicle_footprint = - createVehicleFootprint(planner_data_->parameters.vehicle_info); - boost::geometry::model::ring shifted_vehicle_footprint; - for (const auto & cl : current_lanes) { - // get left and right bounds of current lane - const auto lane_left_bound = cl.leftBound2d().basicLineString(); - const auto lane_right_bound = cl.rightBound2d().basicLineString(); - for (size_t i = start_idx; i < end_idx; ++i) { - // transform vehicle footprint onto path points - shifted_vehicle_footprint = transformVector( - local_vehicle_footprint, - tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose)); - if ( - boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) || - boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) { - if (segment_shift_length > 0.0) { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } - } - } - } - if (ego_front_to_shift_start > 0.0) { - turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; - } else { - turn_signal_info.desired_start_point = blinker_start_pose; - } - turn_signal_info.desired_end_point = blinker_end_pose; - turn_signal_info.required_start_point = blinker_start_pose; - turn_signal_info.required_end_point = blinker_end_pose; - - return turn_signal_info; -} - void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const { using marker_utils::avoidance_marker::createTargetObjectsMarkerArray; @@ -1451,18 +1485,19 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto variable = helper_->getMinAvoidanceDistance( + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto avoid_margin = lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; + const auto avoidance_distance = helper_->getMinAvoidanceDistance( helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + p->stop_buffer; - - return object.longitudinal - std::min(variable + constant, p->stop_max_distance); + const auto constant_distance = helper_->getFrontConstantDistance(object); + + return object.longitudinal - + std::min( + avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer, + p->stop_max_distance); } void AvoidanceModule::insertReturnDeadLine( @@ -1489,7 +1524,8 @@ void AvoidanceModule::insertReturnDeadLine( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); - const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length); + const auto min_return_distance = + helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance(); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point @@ -1545,6 +1581,11 @@ void AvoidanceModule::insertWaitPoint( { const auto & data = avoid_data_; + // If avoidance path is NOT valid, don't insert any stop points. + if (!data.valid) { + return; + } + if (!data.stop_target_object) { return; } @@ -1631,28 +1672,20 @@ void AvoidanceModule::insertStopPoint( getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_); } -void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const +void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const { - const auto & p = parameters_; const auto & data = avoid_data_; - if (data.target_objects.empty()) { + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.safe && !data.stop_target_object) { return; } - if (helper_->isShifted()) { + // If avoidance path is NOT safe, don't insert any slow down sections. + if (!data.valid) { return; } - const auto decel_distance = helper_->getFeasibleDecelDistance(p->yield_velocity, false); - utils::avoidance::insertDecelPoint( - getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_); -} - -void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const -{ - const auto & data = avoid_data_; - // do nothing if there is no avoidance target. if (data.target_objects.empty()) { return; @@ -1668,26 +1701,48 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const return; } - const auto object = data.target_objects.front(); + const auto itr = std::find_if( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return o.avoid_required; }); + + const auto object = [&]() -> std::optional { + if (!data.yield_required) { + return data.target_objects.front(); + } + + if (itr == data.target_objects.end()) { + return std::nullopt; + } + + return *itr; + }(); + + // do nothing if it can't avoid at the moment and avoidance is NOT definitely necessary. + if (!object.has_value()) { + return; + } const auto enough_space = - object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + object.value().is_avoidable || object.value().reason == AvoidanceDebugFactor::TOO_LARGE_JERK; if (!enough_space) { return; } // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_type = utils::getHighestProbLabel(object.value().object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto shift_length = - helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); + const auto lateral_hard_margin = object.value().is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; + const auto avoid_margin = lateral_hard_margin * object.value().distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; + const auto shift_length = helper_->getShiftLength( + object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin); // check slow down feasibility const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length); - const auto distance_to_object = object.longitudinal; + const auto distance_to_object = object.value().longitudinal; const auto remaining_distance = distance_to_object - min_avoid_distance; const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front()); if (remaining_distance < decel_distance) { @@ -1695,7 +1750,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const } // decide slow down lower limit. - const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed; + const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed; // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( @@ -1730,6 +1785,52 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const shifted_path.path.points, start_idx, distance_to_object); } +void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const +{ + const auto & data = avoid_data_; + + // do nothing if no shift line is approved. + if (path_shifter_.getShiftLines().empty()) { + return; + } + + // do nothing if there is no avoidance target. + if (data.target_objects.empty()) { + return; + } + + const auto [distance_to_accel_end_point, v_max] = + helper_->getDistanceToAccelEndPoint(shifted_path.path); + if (distance_to_accel_end_point < 1e-3) { + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto accel_distance = distance_to_accel_end_point - distance_from_ego; + if (accel_distance < 0.0) { + break; + } + + const double v_target_square = + v_max * v_max - 2.0 * parameters_->max_acceleration * accel_distance; + if (v_target_square < 1e-3) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = std::max(getEgoSpeed(), std::sqrt(v_target_square)); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target); + } + + slow_pose_ = motion_utils::calcLongitudinalOffsetPose( + shifted_path.path.points, start_idx, distance_to_accel_end_point); +} + std::shared_ptr AvoidanceModule::get_debug_msg_array() const { debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now(); diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp index 4332008b95e70..5a8fd35f6d618 100644 --- a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp @@ -19,7 +19,7 @@ #include -#include +#include namespace behavior_path_planner::utils::avoidance { @@ -81,7 +81,9 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) AvoidLineArray ret{}; for (const auto & outline : outlines) { ret.push_back(outline.avoid_line); - ret.push_back(outline.return_line); + if (outline.return_line.has_value()) { + ret.push_back(outline.return_line.value()); + } std::for_each( outline.middle_lines.begin(), outline.middle_lines.end(), @@ -121,7 +123,6 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( { // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = helper_->getEgoShift(); - const auto & base_link2rear = data_->parameters.base_link2rear; // Calculate feasible shift length const auto get_shift_profile = @@ -138,13 +139,10 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( // calculate remaining distance. const auto prepare_distance = helper_->getNominalPrepareDistance(); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? 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 constant_distance = helper_->getFrontConstantDistance(object); + const auto has_enough_distance = + object.longitudinal > constant_distance + prepare_distance + nominal_avoid_distance; + const auto remaining_distance = object.longitudinal - constant_distance - prepare_distance; const auto avoidance_distance = has_enough_distance ? nominal_avoid_distance : remaining_distance; @@ -183,29 +181,36 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } // prepare distance is not enough. unavoidable. - if (remaining_distance < 1e-3) { + if (avoidance_distance < 1e-3) { object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; return std::nullopt; } // calculate lateral jerk. const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( - avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed()); + avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed()); // relax lateral jerk limit. avoidable. if (required_jerk < helper_->getLateralMaxJerkLimit()) { return std::make_pair(desire_shift_length, avoidance_distance); } + constexpr double LON_DIST_BUFFER = 1e-3; + // avoidance distance is not enough. unavoidable. if (!isBestEffort(parameters_->policy_deceleration)) { - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return std::nullopt; + if (avoidance_distance < helper_->getMinAvoidanceDistance(avoiding_shift) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } else { + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return std::nullopt; + } } // output avoidance path under lateral jerk constraints. const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( - remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); + avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { object.reason = "LessThanExecutionThreshold"; @@ -216,16 +221,28 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift : -1.0 * feasible_relative_shift_length + current_ego_shift; + if ( + avoidance_distance < + helper_->getMinAvoidanceDistance(feasible_shift_length) + LON_DIST_BUFFER) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } + + const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3; + + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto infeasible = - std::abs(feasible_shift_length - object.overhang_dist) < - 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER < + 0.5 * data_->parameters.vehicle_width + lateral_hard_margin; if (infeasible) { RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; return std::nullopt; } - return std::make_pair(feasible_shift_length, avoidance_distance); + return std::make_pair(feasible_shift_length - LAT_DIST_BUFFER, avoidance_distance); }; const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; @@ -257,11 +274,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( } } - // use each object param - const auto object_type = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); + // calculate feasible shift length based on behavior policy const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - if (!feasible_shift_profile.has_value()) { if (o.avoid_required && is_forward_object(o)) { break; @@ -276,12 +290,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidLine al_avoid; { - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front - : 0.0; - const auto offset = - object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; - const auto to_shift_end = o.longitudinal - offset; + const auto constant_distance = helper_->getFrontConstantDistance(o); + const auto to_shift_end = o.longitudinal - constant_distance; const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index); // start point (use previous linear shift length as start shift length.) @@ -317,8 +327,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidLine al_return; { - const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; - const auto to_shift_start = o.longitudinal + offset; + const auto constant_distance = helper_->getRearConstantDistance(o); + const auto to_shift_start = o.longitudinal + constant_distance; // start point al_return.start_shift_length = feasible_shift_profile.value().first; @@ -341,7 +351,30 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( al_return.object_on_right = utils::avoidance::isOnRight(o); } - if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + const bool skip_return_shift = [&]() { + if (!utils::isAllowedGoalModification(data_->route_handler)) { + return false; + } + const auto goal_pose = data_->route_handler->getGoalPose(); + const double goal_longitudinal_distance = + motion_utils::calcSignedArcLength(data.reference_path.points, 0, goal_pose.position); + const bool is_return_shift_to_goal = + std::abs(al_return.end_longitudinal - goal_longitudinal_distance) < + parameters_->object_check_return_pose_distance; + if (is_return_shift_to_goal) { + return true; + } + const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position; + const bool has_object_near_goal = + tier4_autoware_utils::calcDistance2d(goal_pose.position, object_pos) < + parameters_->object_check_goal_distance; + return has_object_near_goal; + }(); + + if (skip_return_shift) { + // if the object is close to goal or ego is about to return near GOAL, do not return + outlines.emplace_back(al_avoid, std::nullopt); + } else if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { outlines.emplace_back(al_avoid, al_return); } else { o.reason = "InvalidShiftLine"; @@ -637,35 +670,43 @@ AvoidOutlines ShiftLineGenerator::applyMergeProcess( auto & next_outline = outlines.at(i); const auto & return_line = last_outline.return_line; - const auto & avoid_line = next_outline.avoid_line; + if (!return_line.has_value()) { + ret.push_back(outlines.at(i)); + break; + } - if (no_conflict(return_line, avoid_line)) { + const auto & avoid_line = next_outline.avoid_line; + if (no_conflict(return_line.value(), avoid_line)) { ret.push_back(outlines.at(i)); continue; } - const auto merged_shift_line = merge(return_line, avoid_line, generateUUID()); + const auto merged_shift_line = merge(return_line.value(), avoid_line, generateUUID()); if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { ret.push_back(outlines.at(i)); continue; } - if (same_side_shift(return_line, avoid_line)) { + if (same_side_shift(return_line.value(), avoid_line)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + if ( + within(return_line.value(), avoid_line.end_idx) && + within(avoid_line, return_line->start_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + if ( + within(return_line.value(), avoid_line.start_idx) && + within(avoid_line, return_line->end_idx)) { last_outline.middle_lines.push_back(merged_shift_line); last_outline.return_line = next_outline.return_line; debug.step1_merged_shift_line.push_back(merged_shift_line); @@ -686,7 +727,10 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( for (auto & outline : ret) { if (outline.middle_lines.empty()) { - const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID()); + const auto new_line = + outline.return_line.has_value() + ? fill(outline.avoid_line, outline.return_line.value(), generateUUID()) + : outline.avoid_line; outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -701,8 +745,11 @@ AvoidOutlines ShiftLineGenerator::applyFillGapProcess( helper_->alignShiftLinesOrder(outline.middle_lines, false); - if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { - const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID()); + if ( + outline.return_line.has_value() && + outline.middle_lines.back().end_longitudinal < outline.return_line->start_longitudinal) { + const auto new_line = + fill(outline.middle_lines.back(), outline.return_line.value(), generateUUID()); outline.middle_lines.push_back(new_line); debug.step1_filled_shift_line.push_back(new_line); } @@ -890,7 +937,7 @@ void ShiftLineGenerator::applySmallShiftFilter( continue; } - if (s.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { continue; } @@ -973,6 +1020,20 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const bool has_candidate_point = !ret.empty(); const bool has_registered_point = last_.has_value(); + if (utils::isAllowedGoalModification(data_->route_handler)) { + const auto has_object_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + data_->route_handler->getGoalPose().position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_object_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "object near goal exists so skip adding return shift"); + return ret; + } + } + const auto exist_unavoidable_object = std::any_of( data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); @@ -1027,6 +1088,21 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( } } + // if last shift line is near the objects, do not add return shift. + if (utils::isAllowedGoalModification(data_->route_handler)) { + const bool has_last_shift_near_goal = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) { + return tier4_autoware_utils::calcDistance2d( + last_sl.end.position, + o.object.kinematics.initial_pose_with_covariance.pose.position) < + parameters_->object_check_goal_distance; + }); + if (has_last_shift_near_goal) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "last shift line is near the objects"); + return ret; + } + } + // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. // Here the return-shift from ego is added for the in case. @@ -1070,8 +1146,11 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( return ret; } - const auto remaining_distance = std::min( - arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); + const auto remaining_distance = + utils::isAllowedGoalModification(data_->route_handler) + ? data.to_return_point + : std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx); @@ -1101,12 +1180,14 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( const double variable_prepare_distance = std::max(nominal_prepare_distance - last_sl_distance, 0.0); - double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance); + double prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance)); double avoid_distance_scaled = nominal_avoid_distance; if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { const auto scale = (remaining_distance - last_sl_distance) / std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); - prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; + prepare_distance_scaled = std::max( + helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance); avoid_distance_scaled *= scale; } @@ -1120,7 +1201,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.end_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; - al.end_longitudinal = arclength_from_ego.at(al.end_idx); + al.end_longitudinal = prepare_distance_scaled; al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; ret.push_back(al); @@ -1134,7 +1215,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine( al.start_idx = utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); al.start = data.reference_path.points.at(al.start_idx).point.pose; - al.start_longitudinal = arclength_from_ego.at(al.start_idx); + al.start_longitudinal = prepare_distance_scaled; al.end_idx = utils::avoidance::findPathIndexFromArclength( arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); al.end = data.reference_path.points.at(al.end_idx).point.pose; @@ -1219,7 +1300,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine( const auto & candidate = shift_lines.at(i); // prevent sudden steering. - if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) { + if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) { break; } diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index fd120bdd8a069..7c3941d4b95da 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -16,34 +16,31 @@ #include "behavior_path_avoidance_module/data_structs.hpp" #include "behavior_path_avoidance_module/utils.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include -#include -#include +#include #include -#include #include -#include +#include +#include -#include +#include #include #include #include -#include #include -#include #include #include #include #include -#include #include #include @@ -53,6 +50,7 @@ namespace behavior_path_planner::utils::avoidance using autoware_perception_msgs::msg::TrafficSignalElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; +using geometry_msgs::msg::TransformStamped; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -65,7 +63,6 @@ using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; -using tier4_autoware_utils::toHexString; using tier4_planning_msgs::msg::AvoidanceDebugFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsg; @@ -181,7 +178,8 @@ geometry_msgs::msg::Polygon createVehiclePolygon( } Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & p_front, const geometry_msgs::msg::Pose & p_back, + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, + const geometry_msgs::msg::Pose & p3, const geometry_msgs::msg::Pose & p4, const geometry_msgs::msg::Polygon & base_polygon) { Polygon2d one_step_polygon{}; @@ -189,7 +187,7 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_front); + geometry_tf.transform = pose2transform(p1); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -200,7 +198,29 @@ Polygon2d createOneStepPolygon( { geometry_msgs::msg::Polygon out_polygon{}; geometry_msgs::msg::TransformStamped geometry_tf{}; - geometry_tf.transform = pose2transform(p_back); + geometry_tf.transform = pose2transform(p2); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p3); + tf2::doTransform(base_polygon, out_polygon, geometry_tf); + + for (const auto & p : out_polygon.points) { + one_step_polygon.outer().push_back(Point2d(p.x, p.y)); + } + } + + { + geometry_msgs::msg::Polygon out_polygon{}; + geometry_msgs::msg::TransformStamped geometry_tf{}; + geometry_tf.transform = pose2transform(p4); tf2::doTransform(base_polygon, out_polygon, geometry_tf); for (const auto & p : out_polygon.points) { @@ -259,13 +279,15 @@ bool isSafetyCheckTargetObjectType( return parameters->object_parameters.at(object_type).is_safety_check_target; } -bool isVehicleTypeObject(const ObjectData & object) +bool isUnknownTypeObject(const ObjectData & object) { const auto object_type = utils::getHighestProbLabel(object.object.classification); + return object_type == ObjectClassification::UNKNOWN; +} - if (object_type == ObjectClassification::UNKNOWN) { - return false; - } +bool isVehicleTypeObject(const ObjectData & object) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); if (object_type == ObjectClassification::PEDESTRIAN) { return false; @@ -278,6 +300,14 @@ bool isVehicleTypeObject(const ObjectData & object) return true; } +bool isMovingObject( + const ObjectData & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + return object.move_time > object_parameter.moving_time_threshold; +} + bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs) @@ -323,6 +353,14 @@ bool isWithinIntersection( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); } +bool isOnEgoLane(const ObjectData & object) +{ + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + return boost::geometry::within( + lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(), + object.overhang_lanelet.polygon2d().basicPolygon()); +} + bool isParallelToEgoLane(const ObjectData & object, const double threshold) { const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; @@ -333,39 +371,51 @@ bool isParallelToEgoLane(const ObjectData & object, const double threshold) return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; } -bool isObjectOnRoadShoulder( - ObjectData & object, const std::shared_ptr & route_handler, +bool isMergingToEgoLane(const ObjectData & object) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose); + + if (isOnRight(object)) { + if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) { + return false; + } + + if (yaw_deviation > M_PI_2) { + return false; + } + } else { + if (yaw_deviation > 0.0 && M_PI_2 > yaw_deviation) { + return false; + } + + if (yaw_deviation < -1.0 * M_PI_2) { + return false; + } + } + + return true; +} + +bool isParkedVehicle( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & route_handler, const std::shared_ptr & parameters) { - using boost::geometry::return_centroid; - using boost::geometry::within; using lanelet::geometry::distance2d; using lanelet::geometry::toArcCoordinates; using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; - // assume that there are no parked vehicles in intersection. - std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + if (object.is_within_intersection) { return false; } - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + const auto centerline_pos = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position; bool is_left_side_parked_vehicle = false; if (!isOnRight(object)) { @@ -387,8 +437,9 @@ bool isObjectOnRoadShoulder( } } - const auto center_to_left_boundary = - distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + const auto center_to_left_boundary = distance2d( + to2D(most_left_lanelet.leftBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); return std::make_pair( center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); @@ -399,7 +450,8 @@ bool isObjectOnRoadShoulder( } const auto arc_coordinates = toArcCoordinates( - to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + to2D(object.overhang_lanelet.centerline().basicLineString()), + to2D(toLaneletPoint(object_pos)).basicPoint()); object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; @@ -425,8 +477,9 @@ bool isObjectOnRoadShoulder( } } - const auto center_to_right_boundary = - distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); + const auto center_to_right_boundary = distance2d( + to2D(most_right_lanelet.rightBound().basicLineString()), + to2D(toLaneletPoint(centerline_pos)).basicPoint()); return std::make_pair( center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); @@ -437,56 +490,44 @@ bool isObjectOnRoadShoulder( } const auto arc_coordinates = toArcCoordinates( - to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + to2D(object.overhang_lanelet.centerline().basicLineString()), + to2D(toLaneletPoint(object_pos)).basicPoint()); object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; is_right_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; } - return is_left_side_parked_vehicle || is_right_side_parked_vehicle; -} - -bool isForceAvoidanceTarget( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - const std::shared_ptr & parameters) -{ - if (!parameters->enable_force_avoidance_for_stopped_vehicle) { - return false; - } - - const auto stop_time_longer_than_threshold = - object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; - - if (!stop_time_longer_than_threshold) { + if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { return false; } - if (object.is_within_intersection) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { return false; } - const auto rh = planner_data->route_handler; - - if ( - !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) && - !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) { - RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane."); - return false; - } + return true; +} +bool isCloseToStopFactor( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & rh = planner_data->route_handler; const auto & ego_pose = planner_data->self_odometry->pose.pose; const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; // force avoidance for stopped vehicle - bool not_parked_object = true; + bool is_close_to_stop_factor = false; // check traffic light const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); { - not_parked_object = + is_close_to_stop_factor = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; } @@ -498,18 +539,97 @@ bool isForceAvoidanceTarget( const auto stop_for_crosswalk = to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; - not_parked_object = not_parked_object || stop_for_crosswalk; + is_close_to_stop_factor = is_close_to_stop_factor || stop_for_crosswalk; } object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); - return !not_parked_object; + return is_close_to_stop_factor; } -bool isSatisfiedWithCommonCondition( +bool isNeverAvoidanceTarget( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto is_moving_distance_longer_than_threshold = + tier4_autoware_utils::calcDistance2d(object.init_pose, object_pose) > + parameters->distance_threshold_for_ambiguous_vehicle; + if (is_moving_distance_longer_than_threshold) { + object.reason = AvoidanceDebugFactor::MOVING_OBJECT; + return true; + } + + if (object.is_within_intersection) { + if (object.behavior == ObjectData::Behavior::NONE) { + object.reason = "ParallelToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it."); + return true; + } + } + + if (object.is_on_ego_lane) { + const auto right_lane = + planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false); + const auto left_lane = + planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false); + if (right_lane.has_value() && left_lane.has_value()) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it."); + return true; + } + } + + if (isCloseToStopFactor(object, data, planner_data, parameters)) { + if (object.is_on_ego_lane && !object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it."); + return true; + } + } + + return false; +} + +bool isObviousAvoidanceTarget( + ObjectData & object, [[maybe_unused]] const AvoidancePlanningData & data, + [[maybe_unused]] const std::shared_ptr & planner_data, + [[maybe_unused]] const std::shared_ptr & parameters) +{ + if (!object.is_within_intersection) { + if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle."); + return true; + } + + if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle."); + return true; + } + } + + if (!object.is_parked) { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + } + + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "MergingToEgoLane"; + } + + return false; +} + +bool isSatisfiedWithCommonCondition( + ObjectData & object, const AvoidancePlanningData & data, const double forward_detection_range, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) { // Step1. filtered by target object type. if (!isAvoidanceTargetObjectType(object.object, parameters)) { @@ -518,9 +638,7 @@ bool isSatisfiedWithCommonCondition( } // Step2. filtered stopped objects. - const auto object_type = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); - if (object.move_time > object_parameter.moving_time_threshold) { + if (filtering_utils::isMovingObject(object, parameters)) { object.reason = AvoidanceDebugFactor::MOVING_OBJECT; return false; } @@ -534,7 +652,7 @@ bool isSatisfiedWithCommonCondition( return false; } - if (object.longitudinal > parameters->object_check_max_forward_distance) { + if (object.longitudinal > forward_detection_range) { object.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; return false; } @@ -555,11 +673,13 @@ bool isSatisfiedWithCommonCondition( return false; } - if ( - object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > - to_goal_distance) { - object.reason = "TooNearToGoal"; - return false; + if (!utils::isAllowedGoalModification(planner_data->route_handler)) { + if ( + object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > + to_goal_distance) { + object.reason = "TooNearToGoal"; + return false; + } } return true; @@ -576,51 +696,86 @@ bool isSatisfiedWithNonVehicleCondition( return false; } + // Object is on center line -> ignore. + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + object.to_centerline = + lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance; + if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + return true; } +ObjectData::Behavior getObjectBehavior( + ObjectData & object, const std::shared_ptr & parameters) +{ + if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { + return ObjectData::Behavior::NONE; + } + + return isMergingToEgoLane(object) ? ObjectData::Behavior::MERGING + : ObjectData::Behavior::DEVIATING; +} + bool isSatisfiedWithVehicleCondition( ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - using boost::geometry::within; + object.behavior = getObjectBehavior(object, parameters); + object.is_on_ego_lane = isOnEgoLane(object); - object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); + if (isNeverAvoidanceTarget(object, data, planner_data, parameters)) { + return false; + } - // from here condition check for vehicle type objects. - if (isForceAvoidanceTarget(object, data, planner_data, parameters)) { + if (isObviousAvoidanceTarget(object, data, planner_data, parameters)) { return true; } - // Object is on center line -> ignore. - if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) { - object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + // from here, filtering for ambiguous vehicle. + + if (!parameters->enable_avoidance_for_ambiguous_vehicle) { + object.reason = "AmbiguousStoppedVehicle"; return false; } - lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); - const auto on_ego_driving_lane = - within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); - if (on_ego_driving_lane) { - if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) { + const auto stop_time_longer_than_threshold = + object.stop_time > parameters->time_threshold_for_ambiguous_vehicle; + if (!stop_time_longer_than_threshold) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + return false; + } + + if (object.is_within_intersection) { + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; + return true; + } + } else { + if (object.behavior == ObjectData::Behavior::MERGING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; return true; - } else { - object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; - return false; } - } - if (!object.is_within_intersection) { - return true; - } + if (object.behavior == ObjectData::Behavior::DEVIATING) { + object.reason = "AmbiguousStoppedVehicle(wait-and-see)"; + object.is_ambiguous = true; + return true; + } - if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { - object.reason = "ParallelToEgoLane"; - return false; + if (object.behavior == ObjectData::Behavior::NONE) { + object.is_ambiguous = false; + return true; + } } - return true; + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + return false; } bool isNoNeedAvoidanceBehavior( @@ -630,8 +785,8 @@ bool isNoNeedAvoidanceBehavior( return false; } - const auto shift_length = - calcShiftLength(isOnRight(object), object.overhang_dist, object.avoid_margin.value()); + const auto shift_length = calcShiftLength( + isOnRight(object), object.overhang_points.front().first, object.avoid_margin.value()); if (!isShiftNecessary(isOnRight(object), shift_length)) { object.reason = "NotNeedAvoidance"; return true; @@ -652,10 +807,13 @@ std::optional getAvoidMargin( const auto & vehicle_width = planner_data->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto max_avoid_margin = lateral_hard_margin * object.distance_factor + + object_parameter.lateral_soft_margin + 0.5 * vehicle_width; + const auto min_avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; const auto soft_lateral_distance_limit = object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; const auto hard_lateral_distance_limit = @@ -674,11 +832,86 @@ std::optional getAvoidMargin( // Step3. nominal case. avoid margin is limited by soft constraint. return std::min(soft_lateral_distance_limit, max_avoid_margin); } + +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data) +{ + using lanelet::utils::to2D; + using tier4_autoware_utils::Point2d; + + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + findNearestIndex(data.reference_path.points, object_pose.position); + const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; + + const auto rh = planner_data->route_handler; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { + return 0.0; + } + + std::vector> intersects; + for (const auto & p1 : object.overhang_points) { + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto p_tmp = + geometry_msgs::build().position(p1.second).orientation(centerline_pose.orientation); + + // TODO(Satoshi OTA): check if the basic point is on right or left of bound. + const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; + + for (size_t i = 1; i < bound.size(); i++) { + { + const auto p2 = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; + const auto opt_intersect = + tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + + if (opt_intersect.has_value()) { + intersects.emplace_back( + calcDistance2d(p1.second, opt_intersect.value()), p1.second, opt_intersect.value()); + break; + } + } + + { + const auto p2 = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -100.0 : 100.0), 0.0).position; + const auto opt_intersect = + tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); + + if (opt_intersect.has_value()) { + intersects.emplace_back( + -1.0 * calcDistance2d(p1.second, opt_intersect.value()), p1.second, + opt_intersect.value()); + break; + } + } + } + } + + std::sort(intersects.begin(), intersects.end(), [](const auto & a, const auto & b) { + return std::get<0>(a) < std::get<0>(b); + }); + + if (intersects.empty()) { + return 0.0; + } + + object.narrowest_place = + std::make_pair(std::get<1>(intersects.front()), std::get<2>(intersects.front())); + + return std::get<0>(intersects.front()); +} } // namespace filtering_utils bool isOnRight(const ObjectData & obj) { - return obj.lateral < 0.0; + if (obj.direction == Direction::NONE) { + throw std::logic_error("object direction is not initialized. something wrong."); + } + + return obj.direction == Direction::RIGHT; } double calcShiftLength( @@ -689,6 +922,44 @@ double calcShiftLength( return std::fabs(shift_length) > 1e-3 ? shift_length : 0.0; } +bool isWithinLanes( + const lanelet::ConstLanelets & lanelets, std::shared_ptr & planner_data) +{ + const auto & rh = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto transform = tier4_autoware_utils::pose2transform(ego_pose); + const auto footprint = tier4_autoware_utils::transformVector( + planner_data->parameters.vehicle_info.createFootprint(), transform); + + lanelet::ConstLanelet closest_lanelet{}; + if (!lanelet::utils::query::getClosestLanelet(lanelets, ego_pose, &closest_lanelet)) { + return true; + } + + lanelet::ConstLanelets concat_lanelets{}; + + // push previous lanelet + lanelet::ConstLanelets prev_lanelet; + if (rh->getPreviousLaneletsWithinRoute(closest_lanelet, &prev_lanelet)) { + concat_lanelets.push_back(prev_lanelet.front()); + } + + // push nearest lanelet + { + concat_lanelets.push_back(closest_lanelet); + } + + // push next lanelet + lanelet::ConstLanelet next_lanelet; + if (rh->getNextLaneletWithinRoute(closest_lanelet, &next_lanelet)) { + concat_lanelets.push_back(next_lanelet); + } + + const auto combine_lanelet = lanelet::utils::combineLaneletsShape(concat_lanelets); + + return boost::geometry::within(footprint, combine_lanelet.polygon2d().basicPolygon()); +} + bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length) { /** @@ -824,34 +1095,22 @@ void fillLongitudinalAndLengthByClosestEnvelopeFootprint( return; } -double calcEnvelopeOverhangDistance( - const ObjectData & object_data, const PathWithLaneId & path, Point & overhang_pose) +std::vector> calcEnvelopeOverhangDistance( + const ObjectData & object_data, const PathWithLaneId & path) { - double largest_overhang = isOnRight(object_data) ? -100.0 : 100.0; // large number + std::vector> overhang_points{}; for (const auto & p : object_data.envelope_poly.outer()) { const auto point = tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0); // TODO(someone): search around first position where the ego should avoid the object. const auto idx = findNearestIndex(path.points, point); const auto lateral = calcLateralDeviation(getPose(path.points.at(idx)), point); - - const auto & overhang_pose_on_right = [&overhang_pose, &largest_overhang, &point, &lateral]() { - if (lateral > largest_overhang) { - overhang_pose = point; - } - return std::max(largest_overhang, lateral); - }; - - const auto & overhang_pose_on_left = [&overhang_pose, &largest_overhang, &point, &lateral]() { - if (lateral < largest_overhang) { - overhang_pose = point; - } - return std::min(largest_overhang, lateral); - }; - - largest_overhang = isOnRight(object_data) ? overhang_pose_on_right() : overhang_pose_on_left(); + overhang_points.emplace_back(lateral, point); } - return largest_overhang; + std::sort(overhang_points.begin(), overhang_points.end(), [&](const auto & a, const auto & b) { + return isOnRight(object_data) ? b.first < a.first : a.first < b.first; + }); + return overhang_points; } void setEndData( @@ -934,24 +1193,21 @@ std::vector generateObstaclePolygonsForDrivableArea( { std::vector obstacles_for_drivable_area; - if (objects.empty() || !parameters->enable_bound_clipping) { + if (objects.empty()) { return obstacles_for_drivable_area; } for (const auto & object : objects) { - // If avoidance is executed by both behavior and motion, only non-avoidable object will be - // extracted from the drivable area. - if (!parameters->disable_path_update) { - if (object.is_avoidable) { - continue; - } - } - // check if avoid marin is calculated if (!object.avoid_margin.has_value()) { continue; } + // check original polygon + if (object.envelope_poly.outer().empty()) { + continue; + } + const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); @@ -960,55 +1216,12 @@ std::vector generateObstaclePolygonsForDrivableArea( object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); - const bool is_left = 0 < object.lateral; obstacles_for_drivable_area.push_back( - {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, is_left}); + {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)}); } return obstacles_for_drivable_area; } -double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v) -{ - return v * std::cos(calcYawDeviation(p_ref, p_target)); -} - -lanelet::ConstLanelets getTargetLanelets( - const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, - const double left_offset, const double right_offset) -{ - const auto & rh = planner_data->route_handler; - - lanelet::ConstLanelets target_lanelets{}; - for (const auto & lane : route_lanelets) { - auto l_offset = 0.0; - auto r_offset = 0.0; - - const auto opt_left_lane = rh->getLeftLanelet(lane); - if (opt_left_lane) { - target_lanelets.push_back(opt_left_lane.value()); - } else { - l_offset = left_offset; - } - - const auto opt_right_lane = rh->getRightLanelet(lane); - if (opt_right_lane) { - target_lanelets.push_back(opt_right_lane.value()); - } else { - r_offset = right_offset; - } - - const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); - if (!right_opposite_lanes.empty()) { - target_lanelets.push_back(right_opposite_lanes.front()); - } - - const auto expand_lane = lanelet::utils::getExpandedLanelet(lane, l_offset, r_offset); - target_lanelets.push_back(expand_lane); - } - - return target_lanelets; -} - lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data) { @@ -1016,11 +1229,13 @@ lanelet::ConstLanelets getCurrentLanesFromPath( throw std::logic_error("empty path."); } - if (path.points.front().lane_ids.empty()) { + const auto idx = planner_data->findEgoIndex(path.points); + + if (path.points.at(idx).lane_ids.empty()) { throw std::logic_error("empty lane ids."); } - const auto start_id = path.points.front().lane_ids.front(); + const auto start_id = path.points.at(idx).lane_ids.front(); const auto start_lane = planner_data->route_handler->getLaneletsFromId(start_id); const auto & p = planner_data->parameters; @@ -1201,14 +1416,17 @@ void fillAvoidanceNecessity( { const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = object_data.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; const auto safety_margin = - 0.5 * vehicle_width + object_parameter.safety_buffer_lateral * object_data.distance_factor; + 0.5 * vehicle_width + lateral_hard_margin * object_data.distance_factor; const auto check_necessity = [&](const auto hysteresis_factor) { - return (isOnRight(object_data) && - std::abs(object_data.overhang_dist) < safety_margin * hysteresis_factor) || + return (isOnRight(object_data) && std::abs(object_data.overhang_points.front().first) < + safety_margin * hysteresis_factor) || (!isOnRight(object_data) && - object_data.overhang_dist < safety_margin * hysteresis_factor); + object_data.overhang_points.front().first < safety_margin * hysteresis_factor); }; const auto id = object_data.object.object_id; @@ -1232,6 +1450,22 @@ void fillAvoidanceNecessity( object_data.avoid_required = check_necessity(parameters->hysteresis_factor_expand_rate); } +void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects) +{ + const auto id = object_data.object.object_id; + const auto same_id_obj = std::find_if( + detected_objects.begin(), detected_objects.end(), + [&id](const auto & o) { return o.object.object_id == id; }); + + if (same_id_obj != detected_objects.end()) { + object_data.init_pose = same_id_obj->init_pose; + return; + } + + object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose; + detected_objects.push_back(object_data); +} + void fillObjectStoppableJudge( ObjectData & object_data, const ObjectDataArray & registered_objects, const double feasible_stop_distance, const std::shared_ptr & parameters) @@ -1355,67 +1589,45 @@ void compensateDetectionLost( } } -double getRoadShoulderDistance( - ObjectData & object, const AvoidancePlanningData & data, - const std::shared_ptr & planner_data, - [[maybe_unused]] const std::shared_ptr & parameters) +void updateRoadShoulderDistance( + AvoidancePlanningData & data, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) { - using lanelet::utils::to2D; - using tier4_autoware_utils::Point2d; - - const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = - findNearestIndex(data.reference_path.points, object_pose.position); - const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose; - - const auto rh = planner_data->route_handler; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { - return 0.0; - } - - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); - const auto & p1_object = object.overhang_pose.position; - const auto p_tmp = - geometry_msgs::build().position(p1_object).orientation(centerline_pose.orientation); - const auto p2_object = - calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? 100.0 : -100.0), 0.0).position; - - // TODO(Satoshi OTA): check if the basic point is on right or left of bound. - const auto bound = isOnRight(object) ? data.left_bound : data.right_bound; - - std::vector intersects; - for (size_t i = 1; i < bound.size(); i++) { - const auto p1_bound = - geometry_msgs::build().x(bound[i - 1].x()).y(bound[i - 1].y()).z(bound[i - 1].z()); - const auto p2_bound = - geometry_msgs::build().x(bound[i].x()).y(bound[i].y()).z(bound[i].z()); - - const auto opt_intersect = - tier4_autoware_utils::intersect(p1_object, p2_object, p1_bound, p2_bound); - - if (!opt_intersect) { - continue; + ObjectDataArray clip_objects; + std::for_each(data.other_objects.begin(), data.other_objects.end(), [&](const auto & object) { + if (!filtering_utils::isMovingObject(object, parameters)) { + clip_objects.push_back(object); } + }); + for (auto & o : clip_objects) { + const auto & vehicle_width = planner_data->parameters.vehicle_width; + const auto object_type = utils::getHighestProbLabel(o.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + const auto lateral_hard_margin = o.is_parked + ? object_parameter.lateral_hard_margin_for_parked_vehicle + : object_parameter.lateral_hard_margin; - intersects.push_back(opt_intersect.value()); - } - - if (intersects.empty()) { - return 0.0; + o.avoid_margin = lateral_hard_margin + 0.5 * vehicle_width; } + const auto extract_obstacles = generateObstaclePolygonsForDrivableArea( + clip_objects, parameters, planner_data->parameters.vehicle_width / 2.0); - std::sort(intersects.begin(), intersects.end(), [&p1_object](const auto & a, const auto & b) { - return calcDistance2d(p1_object, a) < calcDistance2d(p1_object, b); - }); + auto tmp_path = data.reference_path; + tmp_path.left_bound = data.left_bound; + tmp_path.right_bound = data.right_bound; + utils::extractObstaclesFromDrivableArea(tmp_path, extract_obstacles); - object.nearest_bound_point = intersects.front(); + data.left_bound = tmp_path.left_bound; + data.right_bound = tmp_path.right_bound; - return calcDistance2d(p1_object, object.nearest_bound_point.value()); + for (auto & o : data.target_objects) { + o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + } } void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, + ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -1430,25 +1642,51 @@ void filterTargetObjects( }; for (auto & o : objects) { - if (!filtering_utils::isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { + if (!filtering_utils::isSatisfiedWithCommonCondition( + o, data, forward_detection_range, planner_data, parameters)) { data.other_objects.push_back(o); continue; } - o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters); - o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + // Find the footprint point closest to the path, set to object_data.overhang_distance. + o.overhang_points = utils::avoidance::calcEnvelopeOverhangDistance(o, data.reference_path); + o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data); - if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { - data.other_objects.push_back(o); - continue; + // TODO(Satoshi Ota) parametrize stop time threshold if need. + constexpr double STOP_TIME_THRESHOLD = 3.0; // [s] + if (filtering_utils::isUnknownTypeObject(o)) { + if (o.stop_time < STOP_TIME_THRESHOLD) { + o.reason = "UnstableObject"; + data.other_objects.push_back(o); + continue; + } } if (filtering_utils::isVehicleTypeObject(o)) { + o.is_within_intersection = + filtering_utils::isWithinIntersection(o, planner_data->route_handler); + o.is_parked = + filtering_utils::isParkedVehicle(o, data, planner_data->route_handler, parameters); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { + data.other_objects.push_back(o); + continue; + } + if (!filtering_utils::isSatisfiedWithVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } } else { + o.is_parked = false; + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); + + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { + data.other_objects.push_back(o); + continue; + } + if (!filtering_utils::isSatisfiedWithNonVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; @@ -1500,7 +1738,9 @@ void fillAdditionalInfoFromLongitudinal( { for (auto & outline : outlines) { fillAdditionalInfoFromLongitudinal(data, outline.avoid_line); - fillAdditionalInfoFromLongitudinal(data, outline.return_line); + if (outline.return_line.has_value()) { + fillAdditionalInfoFromLongitudinal(data, outline.return_line.value()); + } std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) { fillAdditionalInfoFromLongitudinal(data, line); @@ -1561,6 +1801,7 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( } lanelet::ConstLanelets getAdjacentLane( + const lanelet::ConstLanelet & current_lane, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift) { @@ -1569,25 +1810,24 @@ lanelet::ConstLanelets getAdjacentLane( const auto & backward_distance = parameters->safety_check_backward_distance; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; - lanelet::ConstLanelet current_lane; - if (!rh->getClosestLaneletWithinRoute(vehicle_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Satoshi Ota) - } - const auto ego_succeeding_lanes = rh->getLaneletSequence(current_lane, vehicle_pose, backward_distance, forward_distance); lanelet::ConstLanelets lanes{}; + + const auto exist = [&lanes](const auto id) { + const auto itr = std::find_if( + lanes.begin(), lanes.end(), [&id](const auto & lane) { return lane.id() == id; }); + return itr != lanes.end(); + }; + for (const auto & lane : ego_succeeding_lanes) { - const auto opt_left_lane = rh->getLeftLanelet(lane); + const auto opt_left_lane = rh->getLeftLanelet(lane, true, false); if (!is_right_shift && opt_left_lane) { lanes.push_back(opt_left_lane.value()); } - const auto opt_right_lane = rh->getRightLanelet(lane); + const auto opt_right_lane = rh->getRightLanelet(lane, true, false); if (is_right_shift && opt_right_lane) { lanes.push_back(opt_right_lane.value()); } @@ -1595,6 +1835,20 @@ lanelet::ConstLanelets getAdjacentLane( const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); if (is_right_shift && !right_opposite_lanes.empty()) { lanes.push_back(right_opposite_lanes.front()); + + for (const auto & prev_lane : rh->getPreviousLanelets(right_opposite_lanes.front())) { + if (!exist(prev_lane.id())) { + lanes.push_back(prev_lane); + } + } + } + } + + for (const auto & lane : lanes) { + for (const auto & next_lane : rh->getNextLanelets(lane)) { + if (!exist(next_lane.id())) { + lanes.push_back(next_lane); + } } } @@ -1603,13 +1857,14 @@ lanelet::ConstLanelets getAdjacentLane( std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, - const std::shared_ptr & parameters, const bool is_right_shift) + const std::shared_ptr & parameters, const bool has_left_shift, + const bool has_right_shift, DebugData & debug) { const auto & p = parameters; const auto check_right_lanes = - (is_right_shift && p->check_shift_side_lane) || (!is_right_shift && p->check_other_side_lane); + (has_right_shift && p->check_shift_side_lane) || (has_left_shift && p->check_other_side_lane); const auto check_left_lanes = - (!is_right_shift && p->check_shift_side_lane) || (is_right_shift && p->check_other_side_lane); + (has_left_shift && p->check_shift_side_lane) || (has_right_shift && p->check_other_side_lane); std::vector target_objects; @@ -1628,7 +1883,10 @@ std::vector getSafetyCheckTargetObjects( PredictedObjects ret{}; std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { - ret.objects.push_back(object.object); + // check only moving objects + if (filtering_utils::isMovingObject(object, parameters)) { + ret.objects.push_back(object.object); + } } }); return ret; @@ -1644,9 +1902,16 @@ std::vector getSafetyCheckTargetObjects( return ret; }(); + lanelet::ConstLanelet closest_lanelet; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + if (!lanelet::utils::query::getClosestLanelet( + data.current_lanelets, ego_pose, &closest_lanelet)) { + return {}; + } + // check right lanes if (check_right_lanes) { - const auto check_lanes = getAdjacentLane(planner_data, p, true); + const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, true); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -1661,11 +1926,14 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check left lanes if (check_left_lanes) { - const auto check_lanes = getAdjacentLane(planner_data, p, false); + const auto check_lanes = getAdjacentLane(closest_lanelet, planner_data, p, false); if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( @@ -1680,6 +1948,9 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } // check current lanes @@ -1699,76 +1970,90 @@ std::vector getSafetyCheckTargetObjects( utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } + + debug.safety_check_lanes.insert( + debug.safety_check_lanes.end(), check_lanes.begin(), check_lanes.end()); } return target_objects; } std::pair separateObjectsByPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const double object_check_forward_distance, const bool is_running, DebugData & debug) + const PathWithLaneId & reference_path, const PathWithLaneId & spline_path, + const std::shared_ptr & planner_data, const AvoidancePlanningData & data, + const std::shared_ptr & parameters, + const double object_check_forward_distance, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; + if (reference_path.points.empty() || spline_path.points.empty()) { + return std::make_pair(target_objects, other_objects); + } + double max_offset = 0.0; for (const auto & object_parameter : parameters->object_parameters) { const auto p = object_parameter.second; + const auto lateral_hard_margin = + std::max(p.lateral_hard_margin, p.lateral_hard_margin_for_parked_vehicle); const auto offset = - 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + 2.0 * p.envelope_buffer_margin + lateral_hard_margin + p.lateral_soft_margin; max_offset = std::max(max_offset, offset); } const auto detection_area = createVehiclePolygon(planner_data->parameters.vehicle_info, max_offset); - const auto ego_idx = planner_data->findEgoIndex(path.points); - - Polygon2d attention_area; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - const auto & p_ego_front = path.points.at(i).point.pose; - const auto & p_ego_back = path.points.at(i + 1).point.pose; - - const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); + const auto ego_idx = planner_data->findEgoIndex(reference_path.points); + const auto arc_length_array = + utils::calcPathArcLengthArray(reference_path, 0L, reference_path.points.size(), 0.0); + + const auto points_size = std::min(reference_path.points.size(), spline_path.points.size()); + + std::vector detection_areas; + Pose p_reference_ego_front = reference_path.points.front().point.pose; + Pose p_spline_ego_front = spline_path.points.front().point.pose; + double next_longitudinal_distance = parameters->resample_interval_for_output; + for (size_t i = 0; i < points_size; ++i) { + const auto distance_from_ego = calcSignedArcLength(reference_path.points, ego_idx, i); if (distance_from_ego > object_check_forward_distance) { break; } - const auto ego_one_step_polygon = createOneStepPolygon(p_ego_front, p_ego_back, detection_area); - - std::vector unions; - boost::geometry::union_(attention_area, ego_one_step_polygon, unions); - if (!unions.empty()) { - attention_area = unions.front(); - boost::geometry::correct(attention_area); + if (arc_length_array.at(i) < next_longitudinal_distance) { + continue; } + + const auto & p_reference_ego_back = reference_path.points.at(i).point.pose; + const auto & p_spline_ego_back = spline_path.points.at(i).point.pose; + + detection_areas.push_back(createOneStepPolygon( + p_reference_ego_front, p_reference_ego_back, p_spline_ego_front, p_spline_ego_back, + detection_area)); + + p_reference_ego_front = p_reference_ego_back; + p_spline_ego_front = p_spline_ego_back; + + next_longitudinal_distance += parameters->resample_interval_for_output; } - // expand detection area width only when the module is running. - if (is_running) { - constexpr int PER_CIRCLE = 36; - constexpr double MARGIN = 1.0; // [m] - boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); - boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); - boost::geometry::strategy::buffer::side_straight side_strategy; - boost::geometry::model::multi_polygon result; - // Create the buffer of a multi polygon - boost::geometry::buffer( - attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, - circle_strategy); - if (!result.empty()) { - attention_area = result.front(); + std::for_each(detection_areas.begin(), detection_areas.end(), [&](const auto & detection_area) { + debug.detection_areas.push_back(toMsg(detection_area, data.reference_pose.position.z)); + }); + + const auto within_detection_area = [&](const auto & obj_polygon) { + for (const auto & detection_area : detection_areas) { + if (!boost::geometry::disjoint(obj_polygon, detection_area)) { + return true; + } } - } - debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); + return false; + }; const auto objects = planner_data->dynamic_object->objects; std::for_each(objects.begin(), objects.end(), [&](const auto & object) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - if (boost::geometry::disjoint(obj_polygon, attention_area)) { + if (!within_detection_area(obj_polygon)) { other_objects.objects.push_back(object); } else { target_objects.objects.push_back(object); @@ -1778,7 +2063,16 @@ std::pair separateObjectsByPath( return std::make_pair(target_objects, other_objects); } -DrivableLanes generateExpandDrivableLanes( +DrivableLanes generateNotExpandedDrivableLanes(const lanelet::ConstLanelet & lanelet) +{ + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = lanelet; + current_drivable_lanes.right_lane = lanelet; + + return current_drivable_lanes; +} + +DrivableLanes generateExpandedDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -1963,7 +2257,9 @@ double calcDistanceToReturnDeadLine( } // dead line for goal - if (parameters->enable_dead_line_for_goal) { + if ( + !utils::isAllowedGoalModification(planner_data->route_handler) && + parameters->enable_dead_line_for_goal) { if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { const auto & ego_pos = planner_data->self_odometry->pose.pose.position; const auto to_goal_distance = diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 58bf36d978cea..8cf85554e2b2c 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,14 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" -#include +#include +#include +#include -#include #include using behavior_path_planner::BehaviorPathPlannerNode; diff --git a/planning/behavior_path_avoidance_module/test/test_utils.cpp b/planning/behavior_path_avoidance_module/test/test_utils.cpp index e5c6fd2e72092..95488daa69cf8 100644 --- a/planning/behavior_path_avoidance_module/test/test_utils.cpp +++ b/planning/behavior_path_avoidance_module/test/test_utils.cpp @@ -25,7 +25,7 @@ using behavior_path_planner::utils::avoidance::isSameDirectionShift; TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) { ObjectData right_obj; - right_obj.lateral = -0.3; + right_obj.direction = route_handler::Direction::RIGHT; const double negative_shift_length = -1.0; const double positive_shift_length = 1.0; @@ -33,11 +33,7 @@ TEST(BehaviorPathPlanningAvoidanceUtilsTest, shiftLengthDirectionTest) ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), positive_shift_length)); ObjectData left_obj; - left_obj.lateral = 0.3; + left_obj.direction = route_handler::Direction::LEFT; ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), positive_shift_length)); ASSERT_FALSE(isSameDirectionShift(isOnRight(left_obj), negative_shift_length)); - - const double zero_shift_length = 0.0; - ASSERT_TRUE(isSameDirectionShift(isOnRight(left_obj), zero_shift_length)); - ASSERT_FALSE(isSameDirectionShift(isOnRight(right_obj), zero_shift_length)); } diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index ff5be388704da..434988cc3ab08 100644 --- a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -14,8 +14,8 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index e90162c958be5..12f8b7075bb14 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -37,6 +37,9 @@ #include #include +#include + +#include #include #include #include @@ -452,7 +455,7 @@ class GoalPlannerModule : public SceneModuleInterface void setDrivableAreaInfo(BehaviorModuleOutput & output) const; // output setter - void setOutput(BehaviorModuleOutput & output) const; + void setOutput(BehaviorModuleOutput & output); void setStopPath(BehaviorModuleOutput & output) const; void updatePreviousData(const BehaviorModuleOutput & output); @@ -467,10 +470,15 @@ class GoalPlannerModule : public SceneModuleInterface */ void setStopPathFromCurrentPath(BehaviorModuleOutput & output) const; void setModifiedGoal(BehaviorModuleOutput & output) const; - void setTurnSignalInfo(BehaviorModuleOutput & output) const; + void setTurnSignalInfo(BehaviorModuleOutput & output); // new turn signal - TurnSignalInfo calcTurnSignalInfo() const; + TurnSignalInfo calcTurnSignalInfo(); + std::optional ignore_signal_{std::nullopt}; + + std::optional last_previous_module_output_{}; + bool hasPreviousModulePathShapeChanged() const; + bool hasDeviatedFromLastPreviousModulePath() const; // timer for generating pull over path candidates in a separate thread void onTimer(); diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ddd7c93998654..9c51623bbf24f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -119,6 +119,11 @@ class PullOverPlannerBase } virtual ~PullOverPlannerBase() = default; + void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + { + previous_module_output_ = previous_module_output; + } + void setPlannerData(const std::shared_ptr planner_data) { planner_data_ = planner_data; @@ -132,6 +137,8 @@ class PullOverPlannerBase vehicle_info_util::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; + + BehaviorModuleOutput previous_module_output_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index bd19dc2e87de0..892ef7d5dd303 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -43,6 +43,8 @@ class ShiftPullOver : public PullOverPlannerBase protected: PathWithLaneId generateReferencePath( const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + std::optional cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 791c35f3afca6..a1fd063bd4116 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -52,8 +52,21 @@ PredictedObjects filterObjectsByLateralDistance( const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects, const double distance_thresh, const bool filter_inside); -bool isAllowedGoalModification(const std::shared_ptr & route_handler); -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path); +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_thresh); + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose); +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const double extend_length); +PathWithLaneId extendPath( + const PathWithLaneId & prev_module_path, const PathWithLaneId & reference_path, + const Pose & extend_pose); // debug MarkerArray createPullOverAreaMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 2ea6f1e100e91..0c6f2768542f1 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -136,11 +136,60 @@ void GoalPlannerModule::updateOccupancyGrid() occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); } +bool GoalPlannerModule::hasPreviousModulePathShapeChanged() const +{ + constexpr double LATERAL_DEVIATION_THRESH = 0.3; + if (!last_previous_module_output_) { + return true; + } + + // Calculate the lateral distance between each point of the current path and the nearest point of + // the last path + const auto current_path = getPreviousModuleOutput().path; + for (const auto & p : current_path.points) { + const size_t nearest_seg_idx = motion_utils::findNearestSegmentIndex( + last_previous_module_output_->path.points, p.point.pose.position); + const auto seg_front = last_previous_module_output_->path.points.at(nearest_seg_idx); + const auto seg_back = last_previous_module_output_->path.points.at(nearest_seg_idx + 1); + // Check if the target point is within the segment + const Eigen::Vector3d segment_vec{ + seg_back.point.pose.position.x - seg_front.point.pose.position.x, + seg_back.point.pose.position.y - seg_front.point.pose.position.y, 0.0}; + const Eigen::Vector3d target_vec{ + p.point.pose.position.x - seg_front.point.pose.position.x, + p.point.pose.position.y - seg_front.point.pose.position.y, 0.0}; + const double dot_product = segment_vec.x() * target_vec.x() + segment_vec.y() * target_vec.y(); + const double segment_length_squared = + segment_vec.x() * segment_vec.x() + segment_vec.y() * segment_vec.y(); + if (dot_product < 0 || dot_product > segment_length_squared) { + // p.point.pose.position is not within the segment, skip lateral distance check + continue; + } + const double lateral_distance = std::abs(motion_utils::calcLateralOffset( + last_previous_module_output_->path.points, p.point.pose.position, nearest_seg_idx)); + if (lateral_distance > LATERAL_DEVIATION_THRESH) { + return true; + } + } + return false; +} + +bool GoalPlannerModule::hasDeviatedFromLastPreviousModulePath() const +{ + constexpr double LATERAL_DEVIATION_THRESH = 0.3; + + if (!last_previous_module_output_) { + return true; + } + return std::abs(motion_utils::calcLateralOffset( + last_previous_module_output_->path.points, + planner_data_->self_odometry->pose.pose.position)) > LATERAL_DEVIATION_THRESH; +} + // generate pull over candidate paths void GoalPlannerModule::onTimer() { - // already generated pull over candidate paths - if (!thread_safe_data_.get_pull_over_path_candidates().empty()) { + if (getCurrentStatus() == ModuleStatus::IDLE) { return; } @@ -149,16 +198,30 @@ void GoalPlannerModule::onTimer() return; } - if ( - !planner_data_ || - !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!planner_data_ || !utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } - if (getCurrentStatus() == ModuleStatus::IDLE) { + // check if new pull over path candidates are needed to be generated + const bool need_update = std::invoke([&]() { + if (thread_safe_data_.get_pull_over_path_candidates().empty()) { + return true; + } + if (hasPreviousModulePathShapeChanged()) { + RCLCPP_ERROR(getLogger(), "has previous module path shape changed"); + return true; + } + if (hasDeviatedFromLastPreviousModulePath() && !hasDecidedPath()) { + RCLCPP_ERROR(getLogger(), "has deviated from last previous module path"); + return true; + } + return false; + }); + if (!need_update) { return; } + const auto previous_module_output = getPreviousModuleOutput(); const auto goal_candidates = thread_safe_data_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose @@ -173,8 +236,9 @@ void GoalPlannerModule::onTimer() const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { planner->setPlannerData(planner_data_); + planner->setPreviousModuleOutput(previous_module_output); auto pull_over_path = planner->plan(goal_candidate.goal_pose); - if (pull_over_path && isCrossingPossible(*pull_over_path)) { + if (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); @@ -188,9 +252,21 @@ void GoalPlannerModule::onTimer() } } }; + + // todo: currently non centerline input path is supported only by shift pull over + const bool is_center_line_input_path = goal_planner_utils::isReferencePath( + previous_module_output.reference_path, previous_module_output.path, 0.1); + RCLCPP_DEBUG( + getLogger(), "the input path of pull over planner is center line: %d", + is_center_line_input_path); + // plan candidate paths and set them to the member variable if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } @@ -198,6 +274,10 @@ void GoalPlannerModule::onTimer() } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { + // todo: temporary skip NON SHIFT planner when input path is not center line + if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { + continue; + } planCandidatePaths(planner, goal_candidate); } } @@ -213,7 +293,10 @@ void GoalPlannerModule::onTimer() const std::lock_guard lock(mutex_); thread_safe_data_.set_pull_over_path_candidates(path_candidates); thread_safe_data_.set_closest_start_pose(closest_start_pose); + RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); } + + last_previous_module_output_ = previous_module_output; } void GoalPlannerModule::onFreespaceParkingTimer() @@ -225,7 +308,7 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } // fixed goal planner do not use freespace planner - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return; } @@ -354,7 +437,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes. lanelet::ConstLanelet current_lane{}; - const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + // const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const lanelet::ConstLanelets current_lanes = + utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, ¤t_lane); const bool goal_is_in_current_lanes = std::any_of( current_lanes.begin(), current_lanes.end(), [&](const lanelet::ConstLanelet & current_lane) { @@ -384,14 +469,14 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (!utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } // if goal arc coordinates can be calculated, check if goal is in request_length const double self_to_goal_arc_length = utils::getSignedDistance(current_pose, goal_pose, current_lanes); - const double request_length = goal_planner_utils::isAllowedGoalModification(route_handler) + const double request_length = utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() : parameters_->pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { @@ -402,7 +487,7 @@ bool GoalPlannerModule::isExecutionRequested() const // if goal modification is not allowed // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner - if (!goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (!utils::isAllowedGoalModification(route_handler)) { return goal_is_in_current_lanes; } @@ -585,7 +670,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const { // calculate goal candidates const auto & route_handler = planner_data_->route_handler; - if (goal_planner_utils::isAllowedGoalModification(route_handler)) { + if (utils::isAllowedGoalModification(route_handler)) { return goal_searcher_->search(); } @@ -603,7 +688,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const BehaviorModuleOutput GoalPlannerModule::plan() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOver(); } @@ -737,7 +822,7 @@ std::vector GoalPlannerModule::generateDrivableLanes() const return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } -void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const +void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { output.reference_path = getPreviousModuleOutput().reference_path; @@ -848,7 +933,7 @@ void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const } } -void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const +void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) { const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); @@ -1094,7 +1179,7 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() { - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { return planPullOverAsCandidate(); } @@ -1153,11 +1238,15 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return PathWithLaneId{}; } - // generate reference path - 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(current_lanes, s_start, s_end, true); + // extend previous module path to generate reference path for stop path + const auto reference_path = std::invoke([&]() -> PathWithLaneId { + 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; + return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); + }); + const auto extended_prev_path = goal_planner_utils::extendPath( + getPreviousModuleOutput().path, reference_path, common_parameters.forward_path_length); // 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 @@ -1165,7 +1254,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const 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, + extended_prev_path.points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); // if not approved stop road lane. @@ -1193,7 +1282,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // 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(extended_prev_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; @@ -1204,55 +1293,43 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const } // slow down for turn signal, insert stop point to stop_pose - decelerateForTurnSignal(*stop_pose, reference_path); + auto stop_path = extended_prev_path; + decelerateForTurnSignal(*stop_pose, stop_path); stop_pose_ = *stop_pose; // for debug wall marker // slow down before the search area. if (decel_pose) { - decelerateBeforeSearchStart(*decel_pose, reference_path); - return reference_path; + decelerateBeforeSearchStart(*decel_pose, stop_path); + return stop_path; } - // if already passed the decel pose, set pull_over_velocity to reference_path. + // if already passed the decel pose, set pull_over_velocity to stop_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); + for (auto & p : stop_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(stop_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; + return stop_path; } 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 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(current_lanes, s_start, s_end, true); - // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); if (!min_stop_distance) { - return stop_path; + return getPreviousModuleOutput().path; } // set stop point + auto stop_path = getPreviousModuleOutput().path; + const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto stop_idx = motion_utils::insertStopPoint(current_pose, *min_stop_distance, stop_path.points); if (stop_idx) { @@ -1382,43 +1459,98 @@ bool GoalPlannerModule::isOnModifiedGoal() const parameters_->th_arrived_distance; } -TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const +TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() { - TurnSignalInfo turn_signal{}; // output + const auto path = thread_safe_data_.get_pull_over_path()->getFullPath(); + if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; - const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); - // calc TurnIndicatorsCommand - { - const double distance_to_end = - calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); - const bool is_before_end_pose = distance_to_end >= 0.0; - if (is_before_end_pose) { - if (left_side_parking_) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + const auto shift_start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); + const auto shift_end_idx = motion_utils::findNearestIndex(path.points, end_pose.position); + + const auto is_ignore_signal = [this](const lanelet::Id & id) { + if (!ignore_signal_.has_value()) { + return false; } + return ignore_signal_.value() == id; + }; + + const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + return is_ignore ? std::make_optional(id) : std::nullopt; + }; + + 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 {}; } - // calc desired/required start/end point - { - // 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_ && 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; + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet); + + if (is_ignore_signal(closest_lanelet.id())) { + return getPreviousModuleOutput().turn_signal_info; } - return turn_signal; + const double current_shift_length = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + constexpr bool egos_lane_is_shifted = true; + constexpr bool is_driving_forward = true; + + constexpr bool is_pull_out = false; + const bool override_ego_stopped_check = std::invoke([&]() { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { + return false; + } + constexpr double distance_threshold = 1.0; + const auto stop_point = + thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); + const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); + return distance_from_ego_to_stop_point < distance_threshold; + }); + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, is_driving_forward, + egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); + + return new_signal; + // // calc TurnIndicatorsCommand + // { + // const double distance_to_end = + // calcSignedArcLength(full_path.points, current_pose.position, end_pose.position); + // const bool is_before_end_pose = distance_to_end >= 0.0; + // if (is_before_end_pose) { + // if (left_side_parking_) { + // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + // } else { + // turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + // } + // } else { + // turn_signal.turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + // } + // } + + // // calc desired/required start/end point + // { + // // 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_ && 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; + // } + + // return turn_signal; } bool GoalPlannerModule::checkOccupancyGridCollision(const PathWithLaneId & path) const @@ -1881,7 +2013,7 @@ void GoalPlannerModule::setDebugData() } tier4_autoware_utils::appendMarkerArray(added, &debug_marker_); }; - if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas const auto color = hasDecidedPath() ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green @@ -1894,6 +2026,14 @@ void GoalPlannerModule::setDebugData() add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } + // Visualize previous module output + add(createPathMarkerArray( + getPreviousModuleOutput().path, "previous_module_path", 0, 1.0, 0.0, 0.0)); + if (last_previous_module_output_.has_value()) { + add(createPathMarkerArray( + last_previous_module_output_.value().path, "last_previous_module_path", 0, 0.0, 1.0, 1.0)); + } + // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( diff --git a/planning/behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_goal_planner_module/src/manager.cpp index 3079361253c31..8608d6cce2c81 100644 --- a/planning/behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_goal_planner_module/src/manager.cpp @@ -419,7 +419,7 @@ bool GoalPlannerModuleManager::isAlwaysExecutableModule() const { // enable AlwaysExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -434,7 +434,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } @@ -449,7 +449,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const // enable SimultaneousExecutable whenever goal modification is not allowed // because only minor path refinements are made for fixed goals - if (!goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + if (!utils::isAllowedGoalModification(planner_data_->route_handler)) { return true; } diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 712da5aa03a4e..b168da61e6239 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -46,10 +46,10 @@ std::optional ShiftPullOver::plan(const Pose & goal_pose) const int shift_sampling_num = parameters_.shift_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; - // get road and shoulder lanes - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_search_length, forward_search_length, + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + previous_module_output_.path, planner_data_, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, backward_search_length, forward_search_length); if (road_lanes.empty() || pull_over_lanes.empty()) { @@ -99,6 +99,31 @@ PathWithLaneId ShiftPullOver::generateReferencePath( return road_lane_reference_path; } +std::optional ShiftPullOver::cropPrevModulePath( + const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const +{ + // clip previous module path to shift end pose nearest segment index + const size_t shift_end_idx = + motion_utils::findNearestSegmentIndex(prev_module_path.points, shift_end_pose.position); + std::vector clipped_points{ + prev_module_path.points.begin(), prev_module_path.points.begin() + shift_end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected shift end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength( + prev_module_path.points, shift_end_idx, shift_end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_prev_module_path = prev_module_path; + clipped_prev_module_path.points = clipped_points; + + return clipped_prev_module_path; +} + std::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -106,33 +131,55 @@ std::optional ShiftPullOver::generatePullOverPath( const double pull_over_velocity = parameters_.pull_over_velocity; const double after_shift_straight_distance = parameters_.after_shift_straight_distance; + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy const Pose shift_end_pose = tier4_autoware_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); - // generate road lane reference path to shift end + // calculate lateral shift of previous module path terminal pose from road lane reference path const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); + const auto prev_module_path = utils::resamplePathWithSpline( + previous_module_output_.path, parameters_.center_line_path_interval); + const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; + + // process previous module path for path shifter input path + // case1) extend path if shift end pose is behind of previous module path terminal pose + // case2) crop path if shift end pose is ahead of previous module path terminal pose + const auto processed_prev_module_path = std::invoke([&]() -> std::optional { + const bool extend_previous_module_path = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > + lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; + if (extend_previous_module_path) { // case1 + return goal_planner_utils::extendPath( + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose); + } else { // case2 + return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); + } + }); + if (!processed_prev_module_path || processed_prev_module_path->points.empty()) { + return {}; + } // calculate shift length - const Pose & shift_end_pose_road_lane = - road_lane_reference_path_to_shift_end.points.back().point.pose; + const Pose & shift_end_pose_prev_module_path = + processed_prev_module_path->points.back().point.pose; const double shift_end_road_to_target_distance = - tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + tier4_autoware_utils::inverseTransformPoint( + shift_end_pose.position, shift_end_pose_prev_module_path) .y; // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( - road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( - road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + processed_prev_module_path->points, shift_end_pose_prev_module_path.position, -before_shifted_pull_over_distance); - if (!shift_start_pose) return {}; // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(road_lane_reference_path_to_shift_end); + path_shifter.setPath(processed_prev_module_path.value()); ShiftLine shift_line{}; shift_line.start = *shift_start_pose; shift_line.end = shift_end_pose; @@ -140,7 +187,9 @@ std::optional ShiftPullOver::generatePullOverPath( path_shifter.addShiftLine(shift_line); ShiftedPath shifted_path{}; const bool offset_back = true; // offset front side from reference path - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + if (!path_shifter.generate(&shifted_path, offset_back)) { + return {}; + } shifted_path.path.points = motion_utils::removeOverlapPoints(shifted_path.path.points); motion_utils::insertOrientation(shifted_path.path.points, true); @@ -208,8 +257,13 @@ std::optional ShiftPullOver::generatePullOverPath( pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; - pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); + pull_over_path.debug_poses.push_back(goal_pose); + pull_over_path.debug_poses.push_back(shift_end_pose); + pull_over_path.debug_poses.push_back( + road_lane_reference_path_to_shift_end.points.back().point.pose); + pull_over_path.debug_poses.push_back(prev_module_path_terminal_pose); // check if the parking path will leave lanes const auto drivable_lanes = @@ -218,8 +272,10 @@ std::optional ShiftPullOver::generatePullOverPath( const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); + const auto combined_drivable = utils::combineDrivableLanes( + expanded_lanes, previous_module_output_.drivable_area_info.drivable_lanes); if (lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(expanded_lanes), pull_over_path.getParkingPath())) { + utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath())) { return {}; } diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 23e9c53efae7b..1610c76c11d50 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -196,22 +196,138 @@ MarkerArray createGoalCandidatesMarkerArray( return marker_array; } -bool isAllowedGoalModification(const std::shared_ptr & route_handler) +double calcLateralDeviationBetweenPaths( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path) { - return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); + double lateral_deviation = 0.0; + for (const auto & target_point : target_path.points) { + const size_t nearest_index = + motion_utils::findNearestIndex(reference_path.points, target_point.point.pose.position); + lateral_deviation = std::max( + lateral_deviation, + std::abs(tier4_autoware_utils::calcLateralDeviation( + reference_path.points[nearest_index].point.pose, target_point.point.pose.position))); + } + return lateral_deviation; +} + +bool isReferencePath( + const PathWithLaneId & reference_path, const PathWithLaneId & target_path, + const double lateral_deviation_threshold) +{ + return calcLateralDeviationBetweenPaths(reference_path, target_path) < + lateral_deviation_threshold; +} + +std::optional cropPath(const PathWithLaneId & path, const Pose & end_pose) +{ + const size_t end_idx = motion_utils::findNearestSegmentIndex(path.points, end_pose.position); + std::vector clipped_points{ + path.points.begin(), path.points.begin() + end_idx}; + if (clipped_points.empty()) { + return std::nullopt; + } + + // add projected end pose to clipped points + PathPointWithLaneId projected_point = clipped_points.back(); + const double offset = motion_utils::calcSignedArcLength(path.points, end_idx, end_pose.position); + projected_point.point.pose = + tier4_autoware_utils::calcOffsetPose(clipped_points.back().point.pose, offset, 0, 0); + clipped_points.push_back(projected_point); + auto clipped_path = path; + clipped_path.points = clipped_points; + + return clipped_path; +} + +PathWithLaneId cropForwardPoints( + const PathWithLaneId & path, const size_t target_seg_idx, const double forward_length) +{ + const auto & points = path.points; + + double sum_length = 0; + for (size_t i = target_seg_idx + 1; i < points.size(); ++i) { + const double seg_length = tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i - 1)); + if (forward_length < sum_length + seg_length) { + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.begin() + i}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + + // add precise end pose to cropped points + const double remaining_length = forward_length - sum_length; + const Pose precise_end_pose = + calcOffsetPose(cropped_path.points.back().point.pose, remaining_length, 0, 0); + if (remaining_length < 0.1) { + // if precise_end_pose is too close, replace the last point + cropped_path.points.back().point.pose = precise_end_pose; + } else { + auto precise_end_point = cropped_path.points.back(); + precise_end_point.point.pose = precise_end_pose; + cropped_path.points.push_back(precise_end_point); + } + return cropped_path; + } + sum_length += seg_length; + } + + // if forward_length is too long, return points after target_seg_idx + const auto cropped_points = + std::vector{points.begin() + target_seg_idx, points.end()}; + PathWithLaneId cropped_path = path; + cropped_path.points = cropped_points; + return cropped_path; } -bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const double extend_length) { - const Pose & goal_pose = route_handler->getOriginalGoalPose(); - const auto shoulder_lanes = route_handler->getShoulderLanelets(); + const auto & target_terminal_pose = target_path.points.back().point.pose; + + // generate clipped road lane reference path from previous module path terminal pose to shift end + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); - lanelet::ConstLanelet closest_shoulder_lane{}; - if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { - return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + PathWithLaneId clipped_path = + cropForwardPoints(reference_path, target_path_terminal_idx, extend_length); + + // shift clipped path to previous module path terminal pose + const double lateral_shift_from_reference_path = + motion_utils::calcLateralOffset(reference_path.points, target_terminal_pose.position); + for (auto & p : clipped_path.points) { + p.point.pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, lateral_shift_from_reference_path, 0); } - return false; + auto extended_path = target_path; + const auto start_point = + std::find_if(clipped_path.points.begin(), clipped_path.points.end(), [&](const auto & p) { + const bool is_forward = + tier4_autoware_utils::inverseTransformPoint(p.point.pose.position, target_terminal_pose).x > + 0.0; + const bool is_close = tier4_autoware_utils::calcDistance2d( + p.point.pose.position, target_terminal_pose.position) < 0.1; + return is_forward && !is_close; + }); + std::copy(start_point, clipped_path.points.end(), std::back_inserter(extended_path.points)); + + extended_path.points = motion_utils::removeOverlapPoints(extended_path.points); + + return extended_path; +} + +PathWithLaneId extendPath( + const PathWithLaneId & target_path, const PathWithLaneId & reference_path, + const Pose & extend_pose) +{ + const auto & target_terminal_pose = target_path.points.back().point.pose; + const size_t target_path_terminal_idx = + motion_utils::findNearestSegmentIndex(reference_path.points, target_terminal_pose.position); + const double extend_distance = motion_utils::calcSignedArcLength( + reference_path.points, target_path_terminal_idx, extend_pose.position); + + return extendPath(target_path, reference_path, extend_distance); } } // namespace goal_planner_utils diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index a1fa2068b9ab5..dc077ab02cae6 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -284,6 +284,29 @@ detach @enduml ``` +To preventive measure for lane change path oscillations caused by alternating safe and unsafe conditions, an additional hysteresis count check is implemented before executing an abort or cancel maneuver. If unsafe, the `unsafe_hysteresis_count_` is incremented and compared against `unsafe_hysteresis_threshold`; exceeding it prompts an abort condition check, ensuring decisions are made with consideration to recent safety assessments as shown in flow chart above. This mechanism stabilizes decision-making, preventing abrupt changes due to transient unsafe conditions. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +title Abort Lane Change + +if (Perform collision check?) then (SAFE) + :Reset unsafe_hysteresis_count_; +else (UNSAFE) + :Increase unsafe_hysteresis_count_; + if (unsafe_hysteresis_count_ > unsafe_hysteresis_threshold?) then (FALSE) + else (TRUE) + #LightPink:Check abort condition; + stop + endif +endif +:Continue lane changing; +@enduml +``` + #### Cancel Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver. @@ -409,6 +432,16 @@ The following parameters are configurable in `behavior_path_planner.param.yaml` | `safety_check.stuck.check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. | true | | `safety_check.stuck.use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------------- | ----- | ------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `enable_collision_check_for_prepare_phase.general_lanes` | [-] | boolean | Perform collision check starting from the prepare phase for situations not explicitly covered by other settings (e.g., intersections). If `false`, collision check only evaluated for lane changing phase. | false | +| `enable_collision_check_for_prepare_phase.intersection` | [-] | boolean | Perform collision check starting from prepare phase when ego is in intersection. If `false`, collision check only evaluated for lane changing phase. | true | +| `enable_collision_check_for_prepare_phase.turns` | [-] | boolean | Perform collision check starting from prepare phase when ego is in lanelet with turn direction tags. If `false`, collision check only evaluated for lane changing phase. | true | +| `prepare_phase_ignore_target_speed_thresh` | [m/s] | double | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true` | 0.1 | +| `check_objects_on_current_lanes` | [-] | boolean | If true, the lane change module check objects on current lanes when performing collision assessment. | false | +| `check_objects_on_other_lanes` | [-] | boolean | If true, the lane change module include objects on other lanes. when performing collision assessment | false | +| `use_all_predicted_path` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | + (\*1) the value must be negative. ### Abort lane change @@ -423,6 +456,7 @@ The following parameters are configurable in `lane_change.param.yaml`. | `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 | | `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 | | `cancel.overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 | +| `unsafe_hysteresis_threshold` | [-] | int | threshold that helps prevent frequent switching between safe and unsafe decisions | 10 | ### Debug diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml index 0cb7f3f1a7a92..1ab33514c5f24 100644 --- a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml @@ -89,7 +89,10 @@ stop_time: 3.0 # [s] # collision check - enable_prepare_segment_collision_check: true + enable_collision_check_for_prepare_phase: + general_lanes: false + intersection: true + turns: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] check_objects_on_current_lanes: true check_objects_on_other_lanes: true @@ -103,6 +106,7 @@ duration: 5.0 # [s] max_lateral_jerk: 1000.0 # [m/s3] overhang_tolerance: 0.0 # [m] + unsafe_hysteresis_threshold: 10 # [/] finish_judge_lateral_threshold: 0.2 # [m] diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 50766621b2f44..77efb658e28c3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -15,6 +15,7 @@ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ #include "behavior_path_lane_change_module/utils/base_class.hpp" +#include "behavior_path_lane_change_module/utils/data_structs.hpp" #include #include @@ -72,6 +73,8 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isApprovedPathSafe() const override; bool isRequiredStop(const bool is_object_coming_from_rear) const override; + PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( + PathSafetyStatus approved_path_safety_status) override; bool isNearEndOfCurrentLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, @@ -141,7 +144,9 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() override; + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; bool isValidPath(const PathWithLaneId & path) const override; @@ -166,6 +171,8 @@ class NormalLaneChange : public LaneChangeBase bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; + bool check_prepare_phase() const; + double calcMaximumLaneChangeLength( const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index aa3ee0dc4c98b..9c891389c4571 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -94,6 +94,9 @@ class LaneChangeBase virtual PathSafetyStatus isApprovedPathSafe() const = 0; + virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis( + PathSafetyStatus approve_path_safety_status) = 0; + virtual bool isNearEndOfCurrentLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const double threshold) const = 0; @@ -225,8 +228,6 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; @@ -250,6 +251,7 @@ class LaneChangeBase PathWithLaneId prev_approved_path_{}; + int unsafe_hysteresis_count_{0}; bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; bool is_activated_{false}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index dd7760d31eaa7..5039dea7c0db6 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -75,6 +75,11 @@ struct LaneChangeCancelParameters double duration{5.0}; double max_lateral_jerk{10.0}; double overhang_tolerance{0.0}; + + // unsafe_hysteresis_threshold will be compare with the number of detected unsafe instance. If the + // number of unsafe exceeds unsafe_hysteresis_threshold, the lane change will be cancelled or + // aborted. + int unsafe_hysteresis_threshold{2}; }; struct LaneChangeParameters @@ -107,7 +112,9 @@ struct LaneChangeParameters double max_longitudinal_acc{1.0}; // collision check - bool enable_prepare_segment_collision_check{true}; + bool enable_collision_check_for_prepare_phase_in_general_lanes{false}; + bool enable_collision_check_for_prepare_phase_in_intersection{true}; + bool enable_collision_check_for_prepare_phase_in_turns{true}; double prepare_segment_ignore_object_velocity_thresh{0.1}; bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; @@ -130,6 +137,7 @@ struct LaneChangeParameters // safety check bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_parked{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; utils::path_safety_checker::RSSparams rss_params_for_stuck{}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 67506326d92aa..a84acf4decd8d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -23,6 +23,7 @@ #include "rclcpp/logger.hpp" #include +#include #include #include @@ -31,6 +32,7 @@ #include +#include #include #include @@ -185,7 +187,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters); + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); @@ -220,5 +222,72 @@ lanelet::ConstLanelets generateExpandedLanelets( * @return rclcpp::Logger The logger instance configured for the specified lane change type. */ rclcpp::Logger getLogger(const std::string & type); + +/** + * @brief Computes the current footprint of the ego vehicle based on its pose and size. + * + * This function calculates the 2D polygon representing the current footprint of the ego vehicle. + * The footprint is determined by the vehicle's pose and its dimensions, including the distance + * from the base to the front and rear ends of the vehicle, as well as its width. + * + * @param ego_pose The current pose of the ego vehicle. + * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal + * offset, rear overhang, and width. + * + * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. + */ +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info); + +/** + * @brief Checks if the given polygon is within an intersection area. + * + * This function evaluates whether a specified polygon is located within the bounds of an + * intersection. It identifies the intersection area by checking the attributes of the provided + * lanelet. If the lanelet has an attribute indicating it is part of an intersection, the function + * then checks if the polygon is fully contained within this area. + * + * @param route_handler a shared pointer to the route_handler + * @param lanelet A lanelet to check against the + * intersection area. + * @param polygon The polygon to check for containment within the intersection area. + * + * @return bool True if the polygon is within the intersection area, false otherwise. + */ +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon); + +/** + * @brief Determines if a polygon is within lanes designated for turning. + * + * Checks if a polygon overlaps with lanelets tagged for turning directions (excluding 'straight'). + * It evaluates the lanelet's 'turn_direction' attribute and determines overlap with the lanelet's + * area. + * + * @param lanelet Lanelet representing the road segment whose turn direction is to be evaluated. + * @param polygon The polygon to be checked for its presence within turn direction lanes. + * + * @return bool True if the polygon is within a lane designated for turning, false if it is within a + * straight lane or no turn direction is specified. + */ +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); + +/** + * @brief Calculates the distance required during a lane change operation. + * + * Used for computing prepare or lane change length based on current and maximum velocity, + * acceleration, and duration, returning the lesser of accelerated distance or distance at max + * velocity. + * + * @param velocity The current velocity of the vehicle in meters per second (m/s). + * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). + * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). + * @param duration The duration of the lane change in seconds (s). + * @return The calculated minimum distance in meters (m). + */ +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double time); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 00793532023b9..61699a8b35bdc 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -116,7 +116,8 @@ ModuleStatus LaneChangeInterface::updateState() return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; } - const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe(); + const auto [is_safe, is_object_coming_from_rear] = + module_type_->evaluateApprovedPathWithUnsafeHysteresis(module_type_->isApprovedPathSafe()); setObjectDebugVisualization(); if (is_safe) { diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index e4405a31a360d..a854a4984d261 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -64,8 +64,12 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) p.max_longitudinal_acc = getOrDeclareParameter(*node, parameter("max_longitudinal_acc")); // collision check - p.enable_prepare_segment_collision_check = - getOrDeclareParameter(*node, parameter("enable_prepare_segment_collision_check")); + p.enable_collision_check_for_prepare_phase_in_general_lanes = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.general_lanes")); + p.enable_collision_check_for_prepare_phase_in_intersection = getOrDeclareParameter( + *node, parameter("enable_collision_check_for_prepare_phase.intersection")); + p.enable_collision_check_for_prepare_phase_in_turns = + getOrDeclareParameter(*node, parameter("enable_collision_check_for_prepare_phase.turns")); p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter( *node, parameter("prepare_segment_ignore_object_velocity_thresh")); p.check_objects_on_current_lanes = @@ -112,6 +116,23 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.execution.lateral_distance_max_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_distance_min_threshold")); + p.rss_params_for_parked.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.longitudinal_velocity_delta_time")); + p.rss_params_for_parked.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_front_deceleration")); + p.rss_params_for_parked.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.parked.expected_rear_deceleration")); + p.rss_params_for_parked.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_reaction_time")); + p.rss_params_for_parked.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.parked.rear_vehicle_safety_time_margin")); + p.rss_params_for_parked.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.parked.lateral_distance_max_threshold")); + p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( @@ -211,6 +232,8 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("cancel.max_lateral_jerk")); p.cancel.overhang_tolerance = getOrDeclareParameter(*node, parameter("cancel.overhang_tolerance")); + p.cancel.unsafe_hysteresis_threshold = + getOrDeclareParameter(*node, parameter("cancel.unsafe_hysteresis_threshold")); p.finish_judge_lateral_threshold = getOrDeclareParameter(*node, parameter("finish_judge_lateral_threshold")); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f0972d66416c8..2fe72c8d964d8 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -420,20 +420,27 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; + unsafe_hysteresis_count_ = 0; object_debug_.clear(); } TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() { - TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); - const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( - status_.current_lanes, status_.lane_change_path.shifted_path, - status_.lane_change_path.info.shift_line, getEgoPose(), getEgoTwist().linear.x, - planner_data_->parameters); - turn_signal_info.turn_signal.command = turn_signal_command.command; - - return turn_signal_info; + const auto & pose = getEgoPose(); + const auto & current_lanes = status_.current_lanes; + const auto & shift_line = status_.lane_change_path.info.shift_line; + const auto & shift_path = status_.lane_change_path.shifted_path; + const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; + constexpr bool is_driving_forward = true; + // The getBehaviorTurnSignalInfo method expects the shifted line to be generated off of the ego's + // current lane, lane change is different, so we set this flag to false. + constexpr bool egos_lane_is_shifted = false; + + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + shift_path, shift_line, current_lanes, current_shift_length, is_driving_forward, + egos_lane_is_shifted); + return new_signal; } lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const @@ -802,23 +809,27 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( target_objects.other_lane.reserve(target_obj_index.other_lane.size()); // objects in current lane + const auto is_check_prepare_phase = check_prepare_phase(); for (const auto & obj_idx : target_obj_index.current_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.current_lane.push_back(extended_object); } // objects in target lane for (const auto & obj_idx : target_obj_index.target_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.target_lane.push_back(extended_object); } // objects in other lane for (const auto & obj_idx : target_obj_index.other_lane) { const auto extended_object = utils::lane_change::transform( - objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_); + objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_, + is_check_prepare_phase); target_objects.other_lane.push_back(extended_object); } @@ -878,8 +889,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); - const auto extended_object = - utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); + const auto extended_object = utils::lane_change::transform( + object, common_parameters, *lane_change_parameters_, check_prepare_phase()); const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); @@ -1171,9 +1182,9 @@ bool NormalLaneChange::getLaneChangePaths( }; // get path on original lanes - const auto prepare_velocity = std::max( + const auto prepare_velocity = std::clamp( current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity); + minimum_lane_changing_velocity, getCommonParam().max_vel); // compute actual longitudinal acceleration const double longitudinal_acc_on_prepare = @@ -1237,8 +1248,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; - const auto terminal_lane_changing_velocity = - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; + const auto terminal_lane_changing_velocity = std::min( + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, + getCommonParam().max_vel); utils::lane_change::setPrepareVelocity( prepare_segment, current_velocity, terminal_lane_changing_velocity); @@ -1440,53 +1452,26 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return safety_status; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() +PathSafetyStatus NormalLaneChange::evaluateApprovedPathWithUnsafeHysteresis( + PathSafetyStatus approved_path_safety_status) { - const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { - double accumulated_length = 0.0; - for (size_t i = 0; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > length) { - return path.points.at(i).point.pose; - } - } - - return path.points.front().point.pose; - }; - - const auto & path = status_.lane_change_path; - const auto & shifted_path = path.shifted_path.path; - - TurnSignalInfo turn_signal_info{}; - - // desired start pose = prepare start pose - turn_signal_info.desired_start_point = std::invoke([&]() { - const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; - const auto diff_time = path.info.duration.prepare - blinker_start_duration; - if (diff_time < 1e-5) { - return path.path.points.front().point.pose; + if (!approved_path_safety_status.is_safe) { + ++unsafe_hysteresis_count_; + RCLCPP_DEBUG( + logger_, "%s: Increasing hysteresis count to %d.", __func__, unsafe_hysteresis_count_); + } else { + if (unsafe_hysteresis_count_ > 0) { + RCLCPP_DEBUG(logger_, "%s: Lane change is now SAFE. Resetting hysteresis count.", __func__); } - - const auto current_twist = getEgoTwist(); - const auto diff_length = std::abs(current_twist.linear.x) * diff_time; - return get_blinker_pose(path.path, diff_length); - }); - - // desired end pose - const auto length_ratio = - std::clamp(lane_change_parameters_->length_ratio_for_turn_signal_deactivation, 0.0, 1.0); - const auto desired_end_length = path.info.length.lane_changing * length_ratio; - turn_signal_info.desired_end_point = get_blinker_pose(shifted_path, desired_end_length); - - // required start pose = lane changing start pose - turn_signal_info.required_start_point = path.info.shift_line.start; - - // required end pose = in the middle of the lane change - const auto mid_lane_change_length = path.info.length.lane_changing / 2; - turn_signal_info.required_end_point = get_blinker_pose(shifted_path, mid_lane_change_length); - - return turn_signal_info; + unsafe_hysteresis_count_ = 0; + } + if (unsafe_hysteresis_count_ > lane_change_parameters_->cancel.unsafe_hysteresis_threshold) { + RCLCPP_DEBUG( + logger_, "%s: hysteresis count exceed threshold. lane change is now %s", __func__, + (approved_path_safety_status.is_safe ? "safe" : "UNSAFE")); + return approved_path_safety_status; + } + return {}; } bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const @@ -1730,9 +1715,13 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); auto is_safe = true; + const auto selected_rss_param = + (obj.initial_twist.twist.linear.x <= lane_change_parameters_->stop_velocity_threshold) + ? lane_change_parameters_->rss_params_for_parked + : rss_params; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( - path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, + path, ego_predicted_path, obj, obj_path, common_parameters, selected_rss_param, 1.0, current_debug_data.second); if (collided_polygons.empty()) { @@ -1878,4 +1867,41 @@ void NormalLaneChange::updateStopTime() stop_watch_.tic("stop_time"); } +bool NormalLaneChange::check_prepare_phase() const +{ + const auto & route_handler = getRouteHandler(); + const auto & vehicle_info = getCommonParam().vehicle_info; + + const auto check_in_general_lanes = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; + + lanelet::ConstLanelet current_lane; + if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_DEBUG( + logger_, "Unable to get current lane. Default to %s.", + (check_in_general_lanes ? "true" : "false")); + return check_in_general_lanes; + } + + const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + + const auto check_in_intersection = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { + return false; + } + + return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); + }); + + const auto check_in_turns = std::invoke([&]() { + if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) { + return false; + } + + return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); + }); + + return check_in_intersection || check_in_turns || check_in_general_lanes; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index df73990b770a3..60d7c699dc6bb 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -30,7 +30,13 @@ #include #include #include +#include #include +#include + +#include + +#include #include #include @@ -1099,7 +1105,7 @@ std::optional createPolygon( ExtendedPredictedObject transform( const PredictedObject & object, [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, - const LaneChangeParameters & lane_change_parameters) + const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase) { ExtendedPredictedObject extended_object; extended_object.uuid = object.object_id; @@ -1109,8 +1115,6 @@ ExtendedPredictedObject transform( extended_object.shape = object.shape; const auto & time_resolution = lane_change_parameters.prediction_time_resolution; - const auto & check_at_prepare_phase = - lane_change_parameters.enable_prepare_segment_collision_check; const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; @@ -1168,4 +1172,51 @@ rclcpp::Logger getLogger(const std::string & type) { return rclcpp::get_logger("lane_change").get_child(type); } + +Polygon2d getEgoCurrentFootprint( + const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info) +{ + const auto base_to_front = ego_info.max_longitudinal_offset_m; + const auto base_to_rear = ego_info.rear_overhang_m; + const auto width = ego_info.vehicle_width_m; + + return tier4_autoware_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width); +} + +bool isWithinIntersection( + const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, + const Polygon2d & polygon) +{ + const std::string id = lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + return false; + } + + const auto lanelet_polygon = + route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + return boost::geometry::within( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); +} + +bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) +{ + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "else" || turn_direction == "straight") { + return false; + } + + return !boost::geometry::disjoint( + polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); +} + +double calcPhaseLength( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + velocity * duration + 0.5 * acceleration * std::pow(duration, 2); + const auto length_with_max_velocity = maximum_velocity * duration; + return std::min(length_with_acceleration, length_with_max_velocity); +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 7a606b5d126ce..82f721411d5a4 100644 --- a/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -14,8 +14,8 @@ #include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" +#include "planning_test_utils/planning_interface_test_manager.hpp" +#include "planning_test_utils/planning_interface_test_manager_utils.hpp" #include diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index b4d323de45cde..2bdff139673f0 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -18,6 +18,7 @@ turn_signal_minimum_search_distance: 10.0 turn_signal_search_time: 3.0 turn_signal_shift_length_threshold: 0.3 + turn_signal_remaining_shift_length_threshold: 0.1 turn_signal_on_swerving: true enable_akima_spline_first: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index c4580bb1e82c3..5fc1105967b15 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -833,7 +833,8 @@ namespace: `avoidance.target_filtering.` | object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | | object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | | object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | +| object_check_goal_distance | [m] | double | If the distance between object and goal position is less than this parameter, the module do not return center line. | 20.0 | +| object_check_return_pose_distance | [m] | double | If the distance between object and return position is less than this parameter, the module do not return center line. | 20.0 | | object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | | object_check_min_road_shoulder_width | [m] | double | Width considered as a road shoulder if the lane does not have a road shoulder target. | 0.5 | | object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md index 8b64db997aec6..f0c8f8d8cbb63 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design.md @@ -4,7 +4,7 @@ Turn Signal decider determines necessary blinkers. ## Purpose / Role -This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Raw. +This module is responsible for activating a necessary blinker during driving. It uses rule-based algorithm to determine blinkers, and the details of this algorithm are described in the following sections. Note that this algorithm is strictly based on the Japanese Road Traffic Law. ### Assumptions @@ -16,18 +16,19 @@ Autoware has following order of priorities for turn signals. ### Limitations -Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in a complicated situations. This is because it tries to follow the road traffic raw and cannot solve `blinker conflicts` clearly in that environment. +Currently, this algorithm can sometimes give unnatural (not wrong) blinkers in complicated situations. This is because it tries to follow the road traffic law and cannot solve `blinker conflicts` clearly in that environment. ## Parameters for turn signal decider -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------- | :--- | :----- | :--------------------------------------------------------------------------- | :------------ | -| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | -| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | -| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | -| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | -| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | -| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------ | :------------ | +| turn_signal_intersection_search_distance | [m] | double | constant search distance to decide activation of blinkers at intersections | 30 | +| turn_signal_intersection_angle_threshold_degree | deg | double | angle threshold to determined the end point of intersection required section | 15 | +| turn_signal_minimum_search_distance | [m] | double | minimum search distance for avoidance and lane change | 10 | +| turn_signal_search_time | [s] | double | search time for to decide activation of blinkers | 3.0 | +| turn_signal_shift_length_threshold | [m] | double | shift length threshold to decide activation of blinkers | 0.3 | +| turn_signal_remaining_shift_length_threshold | [m] | double | When the ego's current shift length minus its end shift length is less than this threshold, the turn signal will be turned off. | 0.1 | +| turn_signal_on_swerving | [-] | bool | flag to activate blinkers when swerving | true | Note that the default values for `turn_signal_intersection_search_distance` and `turn_signal_search_time` is strictly followed by [Japanese Road Traffic Laws](https://www.japaneselawtranslation.go.jp/ja/laws/view/2962). So if your country does not allow to use these default values, you should change these values in configuration files. @@ -59,16 +60,16 @@ For left turn, right turn, avoidance, lane change, goal planner and pull out, we Turn signal decider checks each lanelet on the map if it has `turn_direction` information. If a lanelet has this information, it activates necessary blinker based on this information. -- desired start point - The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in gree in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance. +- desired start point + The `search_distance` for blinkers at intersections is `v * turn_signal_search_time + turn_signal_intersection_search_distance`. Then the start point becomes `search_distance` meters before the start point of the intersection lanelet(depicted in green in the following picture), where `v` is the velocity of the ego vehicle. However, if we set `turn_signal_distance` in the lanelet, we use that length as search distance. -- desired end point +- desired end point Terminal point of the intersection lanelet. -- required start point +- required start point Initial point of the intersection lanelet. -- required end point +- required end point The earliest point that satisfies the following condition. $\theta - \theta_{\textrm{end}} < \delta$, where $\theta_{\textrm{end}}$ is yaw angle of the terminal point of the lanelet, $\theta$ is the angle of a required end point and $\delta$ is the threshold defined by the user. ![section_turn_signal](../image/turn_signal_decider/left_right_turn.drawio.svg) @@ -79,100 +80,100 @@ Avoidance can be separated into two sections, first section and second section. First section -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the avoidance shift path. -- desired end point +- desired end point Shift complete point where the path shift is completed. -- required start point +- required start point Avoidance start point. -- required end point +- required end point Shift complete point same as the desired end point. ![section_first_avoidance](../image/turn_signal_decider/avoidance_first_section.drawio.svg) Second section -- desired start point +- desired start point Shift complete point. -- desired end point +- desired end point Avoidance terminal point -- required start point +- required start point Shift complete point. -- required end point +- required end point Avoidance terminal point. ![section_second_avoidance](../image/turn_signal_decider/avoidance_second_section.drawio.svg) #### 3. Lane Change -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the lane change path. -- desired end point +- desired end point Terminal point of the lane change path. -- required start point +- required start point Initial point of the lane change path. -- required end point +- required end point Cross point with lane change path and boundary line of the adjacent lane. ![section_lane_change](../image/turn_signal_decider/lane_change.drawio.svg) #### 4. Pull out -- desired start point +- desired start point Start point of the path of pull out. -- desired end point +- desired end point Terminal point of the path of pull out. -- required start point +- required start point Start point of the path pull out. -- required end point +- required end point Terminal point of the path of pull out. ![section_pull_out](../image/turn_signal_decider/pull_out.drawio.svg) #### 5. Goal Planner -- desired start point +- desired start point `v * turn_signal_search_time` meters before the start point of the pull over path. -- desired end point +- desired end point Terminal point of the path of pull over. -- required start point +- required start point Start point of the path of pull over. -- required end point +- required end point Terminal point of the path of pull over. ![section_pull_over](../image/turn_signal_decider/pull_over.drawio.svg) When it comes to handle several blinkers, it gives priority to the first blinker that comes first. However, this rule sometimes activate unnatural blinkers, so turn signal decider uses the following five rules to decide the necessary turn signal. -- pattern1 - ![pattern1](../image/turn_signal_decider/pattern1.drawio.svg) +- pattern1 + ![pattern1](../images/turn_signal_decider/pattern1.drawio.svg) -- pattern2 - ![pattern2](../image/turn_signal_decider/pattern2.drawio.svg) +- pattern2 + ![pattern2](../images/turn_signal_decider/pattern2.drawio.svg) -- pattern3 - ![pattern3](../image/turn_signal_decider/pattern3.drawio.svg) +- pattern3 + ![pattern3](../images/turn_signal_decider/pattern3.drawio.svg) -- pattern4 - ![pattern4](../image/turn_signal_decider/pattern4.drawio.svg) +- pattern4 + ![pattern4](../images/turn_signal_decider/pattern4.drawio.svg) -- pattern5 - ![pattern5](../image/turn_signal_decider/pattern5.drawio.svg) +- pattern5 + ![pattern5](../images/turn_signal_decider/pattern5.drawio.svg) Based on these five rules, turn signal decider can solve `blinker conflicts`. The following pictures show some examples of this kind of conflicts. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 7ceafb6f428de..3ccc92d04025f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -286,6 +286,8 @@ class PlannerManager module_ptr->publishRTCStatus(); + module_ptr->publishSteeringFactor(); + module_ptr->publishObjectsOfInterestMarker(); processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 85abf774d159e..3c6c6a883715f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -242,6 +242,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.min_acc = declare_parameter("normal.min_acc"); p.max_acc = declare_parameter("normal.max_acc"); + p.max_vel = declare_parameter("max_vel"); p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over"); p.backward_length_buffer_for_end_of_pull_out = @@ -258,6 +259,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_search_time = declare_parameter("turn_signal_search_time"); p.turn_signal_shift_length_threshold = declare_parameter("turn_signal_shift_length_threshold"); + p.turn_signal_remaining_shift_length_threshold = + declare_parameter("turn_signal_remaining_shift_length_threshold"); p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving"); p.enable_akima_spline_first = declare_parameter("enable_akima_spline_first"); diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index e4bfff8fc63b8..fa5a745be9f0e 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -216,8 +216,7 @@ void PlannerManager::generateCombinedDrivableArea( } else if (di.is_already_expanded) { // for single side shift utils::generateDrivableArea( - output.path, di.drivable_lanes, false, false, data->parameters.vehicle_length, data, - is_driving_forward); + output.path, di.drivable_lanes, false, false, false, data, is_driving_forward); } else { const auto shorten_lanes = utils::cutOverlappedLanes(output.path, di.drivable_lanes); @@ -229,7 +228,7 @@ void PlannerManager::generateCombinedDrivableArea( // for other modules where multiple modules may be launched utils::generateDrivableArea( output.path, expanded_lanes, di.enable_expanding_hatched_road_markings, - di.enable_expanding_intersection_areas, data->parameters.vehicle_length, data, + di.enable_expanding_intersection_areas, di.enable_expanding_freespace_areas, data, is_driving_forward); } diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index f2c22f774fc77..28ee6d31e80dc 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index d52906ef4684f..6821b07fb3f4e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -21,7 +21,9 @@ #include "motion_utils/trajectory/trajectory.hpp" #include -#include +#include +#include +#include #include #include @@ -44,6 +46,7 @@ #include #include #include +#include #include namespace behavior_path_planner @@ -106,6 +109,7 @@ struct DrivableAreaInfo std::vector obstacles{}; // obstacles to extract from the drivable area bool enable_expanding_hatched_road_markings{false}; bool enable_expanding_intersection_areas{false}; + bool enable_expanding_freespace_areas{false}; // temporary only for pull over's freespace planning double drivable_margin{0.0}; @@ -164,6 +168,62 @@ struct PlannerData mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; + std::pair getBehaviorTurnSignalInfo( + const PathWithLaneId & path, const size_t shift_start_idx, const size_t shift_end_idx, + const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, + const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + { + if (shift_start_idx + 1 > path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_end_idx + 1 > path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + std::vector lengths(path.points.size(), 0.0); + ShiftedPath shifted_path{path, lengths}; + ShiftLine shift_line; + + { + const auto start_pose = path.points.at(shift_start_idx).point.pose; + const auto start_shift_length = + lanelet::utils::getArcCoordinates(current_lanelets, start_pose).distance; + const auto end_pose = path.points.at(shift_end_idx).point.pose; + const auto end_shift_length = + lanelet::utils::getArcCoordinates(current_lanelets, end_pose).distance; + shifted_path.shift_length.at(shift_start_idx) = start_shift_length; + shifted_path.shift_length.at(shift_end_idx) = end_shift_length; + + shift_line.start = start_pose; + shift_line.end = end_pose; + shift_line.start_shift_length = start_shift_length; + shift_line.end_shift_length = end_shift_length; + shift_line.start_idx = shift_start_idx; + shift_line.end_idx = shift_end_idx; + } + + return turn_signal_decider.getBehaviorTurnSignalInfo( + shifted_path, shift_line, current_lanelets, route_handler, parameters, self_odometry, + current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, + is_pull_out); + } + + std::pair getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, const double current_shift_length, + const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check = false, const bool is_pull_out = false) const + { + return turn_signal_decider.getBehaviorTurnSignalInfo( + path, shift_line, current_lanelets, route_handler, parameters, self_odometry, + current_shift_length, is_driving_forward, egos_lane_is_shifted, override_ego_stopped_check, + is_pull_out); + } + TurnIndicatorsCommand getTurnSignal( const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, TurnSignalDebugData & debug_data) diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 95976bf117096..d76cbc80499be 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -261,7 +261,14 @@ class SceneModuleManagerInterface // init manager configuration { std::string ns = name_ + "."; - config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + try { + config_.enable_rtc = getOrDeclareParameter(*node, "enable_all_modules_auto_mode") + ? false + : getOrDeclareParameter(*node, ns + "enable_rtc"); + } catch (const std::exception & e) { + config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + } + config_.enable_simultaneous_execution_as_approved_module = getOrDeclareParameter(*node, ns + "enable_simultaneous_execution_as_approved_module"); config_.enable_simultaneous_execution_as_candidate_module = getOrDeclareParameter( diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 5bdd0a2f3f88d..c9ebfe18362f9 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -45,6 +45,7 @@ struct BehaviorPathPlannerParameters // common parameters double min_acc; double max_acc; + double max_vel; double minimum_pull_over_length; double minimum_pull_out_length; @@ -57,6 +58,7 @@ struct BehaviorPathPlannerParameters double turn_signal_search_time; double turn_signal_minimum_search_distance; double turn_signal_shift_length_threshold; + double turn_signal_remaining_shift_length_threshold; bool turn_signal_on_swerving; bool enable_akima_spline_first; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index 372041c7fb586..18afecf5feb4f 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -15,14 +15,24 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" + #include +#include +#include #include +#include +#include #include #include #include +#include + +#include #include +#include #include #include @@ -37,6 +47,7 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; using route_handler::RouteHandler; const std::map signal_map = { @@ -100,6 +111,15 @@ class TurnSignalDecider std::pair getIntersectionTurnSignalFlag(); std::pair getIntersectionPoseAndDistance(); + std::pair getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, + const std::shared_ptr route_handler, + const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, + const double current_shift_length, const bool is_driving_forward, + const bool egos_lane_is_shifted, const bool override_ego_stopped_check = false, + const bool is_pull_out = false) const; + private: std::optional getIntersectionTurnSignalInfo( const PathWithLaneId & path, const Pose & current_pose, const double current_vel, @@ -118,6 +138,132 @@ class TurnSignalDecider const double nearest_yaw_threshold); void initialize_intersection_info(); + inline bool isAvoidShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return std::abs(start_shift_length) < threshold && std::abs(end_shift_length) > threshold; + }; + + inline bool isReturnShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + + inline bool isLeftMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return start_shift_length > threshold && end_shift_length > threshold; + }; + + inline bool isRightMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) const + { + return start_shift_length < threshold && end_shift_length < threshold; + }; + + inline bool existShiftSideLane( + const double start_shift_length, const double end_shift_length, const bool no_left_lanes, + const bool no_right_lanes, const double threshold) const + { + const auto relative_shift_length = end_shift_length - start_shift_length; + if (isAvoidShift(start_shift_length, end_shift_length, threshold)) { + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Right avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + } + + if (isReturnShift(start_shift_length, end_shift_length, threshold)) { + // Right return. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + if (isLeftMiddleShift(start_shift_length, end_shift_length, threshold)) { + // Left avoid. But there is no adjacent lane. No need blinker. + + if (relative_shift_length > 0.0 && no_left_lanes) { + return false; + } + + // Left return. But there is no adjacent lane. No need blinker. + if (relative_shift_length < 0.0 && no_left_lanes) { + return false; + } + } + + if (isRightMiddleShift(start_shift_length, end_shift_length, threshold)) { + // Right avoid. But there is no adjacent lane. No need blinker. + + if (relative_shift_length < 0.0 && no_right_lanes) { + return false; + } + + // Left avoid. But there is no adjacent lane. No need blinker. + if (relative_shift_length > 0.0 && no_right_lanes) { + return false; + } + } + return true; + }; + + inline bool straddleRoadBound( + const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, + const vehicle_info_util::VehicleInfo & vehicle_info) const + { + using boost::geometry::intersects; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto footprint = vehicle_info.createFootprint(); + + for (const auto & lane : lanes) { + for (size_t i = shift_line.start_idx; i < shift_line.end_idx; ++i) { + const auto transform = pose2transform(path.path.points.at(i).point.pose); + const auto shifted_vehicle_footprint = transformVector(footprint, transform); + + if (intersects(lane.leftBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + + if (intersects(lane.rightBound2d().basicLineString(), shifted_vehicle_footprint)) { + return true; + } + } + } + + return false; + }; + + inline bool isNearEndOfShift( + const double start_shift_length, const double end_shift_length, const Point & ego_pos, + const lanelet::ConstLanelets & original_lanes, const double threshold) const + { + using boost::geometry::within; + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; + + if (!isReturnShift(start_shift_length, end_shift_length, threshold)) { + return false; + } + + return std::any_of(original_lanes.begin(), original_lanes.end(), [&ego_pos](const auto & lane) { + return within(to2D(toLaneletPoint(ego_pos)), lane.polygon2d().basicPolygon()); + }); + }; + geometry_msgs::msg::Quaternion calc_orientation(const Point & src_point, const Point & dst_point); rclcpp::Logger logger_{ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index 284b4aad20a6a..2e9ad54ae3378 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -19,6 +19,7 @@ #include #include +#include #include namespace behavior_path_planner::utils { @@ -40,8 +41,8 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward = true); + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward = true); void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, @@ -65,6 +66,11 @@ std::vector expandLanelets( void extractObstaclesFromDrivableArea( PathWithLaneId & path, const std::vector & obstacles); +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left); + std::vector getBoundWithHatchedRoadMarkings( const std::vector & original_bound, const std::shared_ptr & route_handler); @@ -75,14 +81,22 @@ std::vector getBoundWithIntersectionAreas( const std::vector & drivable_lanes, const bool is_left); std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool enable_expanding_freespace_areas, const bool is_left, + const bool is_driving_forward = true); + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left); + const bool is_left, const bool is_driving_forward = true); -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left); +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left); DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 4d1f1fdb959c0..3a2465eda442c 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" +#include #include #include @@ -78,7 +79,8 @@ class GeometricParallelParking const bool left_side_parking); bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start); + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, + const std::shared_ptr lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } void setPlannerData(const std::shared_ptr & planner_data) { @@ -121,7 +123,8 @@ class GeometricParallelParking const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, - const double lane_departure_margin, const double arc_path_interval); + const double lane_departure_margin, const double arc_path_interval, + const std::shared_ptr lane_departure_checker); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, const double arc_path_interval, const bool is_left_turn, const bool is_forward); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index d10a2f38fb975..b3c8034a3545c 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -126,6 +126,9 @@ struct TargetObjectsOnLane */ struct RSSparams { + std::string extended_polygon_policy{ + "rectangle"}; ///< Policy to create collision check polygon. + ///< possible values: ["rectangle", "along_path"] double rear_vehicle_reaction_time{0.0}; ///< Reaction time of the rear vehicle. double rear_vehicle_safety_time_margin{0.0}; ///< Safety time margin for the rear vehicle. double lateral_distance_max_threshold{0.0}; ///< Maximum threshold for lateral distance. diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp index bf48252a01dcd..e834443a40a51 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/traffic_light_utils.hpp @@ -129,6 +129,21 @@ bool isStoppedAtRedTrafficLightWithinDistance( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data, const double distance_threshold = std::numeric_limits::infinity()); + +/** + * @brief Determines if a traffic signal indicates a stop for the given lanelet. + * + * Evaluates the current state of the traffic light, considering if it's green or unknown, + * which would not necessitate a stop. Then, it checks the turn direction attribute of the lanelet + * against the traffic light's arrow shapes to determine whether a vehicle must stop or if it can + * proceed based on allowed turn directions. + * + * @param lanelet The lanelet to check for a stop signal at its traffic light. + * @param planner_data Shared pointer to the planner data containing vehicle state information. + * @return True if the traffic signal indicates a stop is required, false otherwise. + */ +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::traffic_light #endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index e2c9fd1e332a2..6a04590d6f6d7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -243,6 +243,9 @@ PathWithLaneId refinePathForGoal( bool containsGoal(const lanelet::ConstLanelets & lanes, const lanelet::Id & goal_id); +bool isAllowedGoalModification(const std::shared_ptr & route_handler); +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler); + bool isInLanelets(const Pose & pose, const lanelet::ConstLanelets & lanes); bool isInLaneletWithYawThreshold( @@ -325,6 +328,10 @@ lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data, const double backward_length, const double forward_length, const bool forward_only_in_route); +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route); + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const geometry_msgs::msg::Pose & pose, const double forward_length, const double backward_length, diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index b289ce86ca60e..3dba32cec2399 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -47,6 +47,7 @@ freespace_planning_algorithms geometry_msgs interpolation + lane_departure_checker lanelet2_extension libboost-dev libopencv-dev diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp index 933330dafae7e..16537620a29a6 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -33,6 +33,8 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; + double calc_distance( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const Pose & input_point, const double nearest_dist_threshold, const double nearest_yaw_threshold) @@ -43,6 +45,11 @@ double calc_distance( path.points, current_pose.position, current_seg_idx, input_point.position, nearest_seg_idx); } +/*** + * @brief: + * Gets the turn signal info after comparing the turn signal info output from the behavior path + * module and comparing it to turn signal info obtained from intersections. + */ TurnIndicatorsCommand TurnSignalDecider::getTurnSignal( const std::shared_ptr & route_handler, const PathWithLaneId & path, const TurnSignalInfo & turn_signal_info, const Pose & current_pose, const double current_vel, @@ -129,6 +136,13 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const size_t current_seg_idx, const RouteHandler & route_handler, const double nearest_dist_threshold, const double nearest_yaw_threshold) { + const auto requires_turn_signal = [¤t_vel]( + const auto & turn_direction, const bool is_in_turn_lane) { + constexpr double stop_velocity_threshold = 0.1; + return ( + turn_direction == "right" || turn_direction == "left" || + (turn_direction == "straight" && current_vel < stop_velocity_threshold && !is_in_turn_lane)); + }; // base search distance const double base_search_distance = intersection_search_time_ * current_vel + intersection_search_distance_; @@ -145,6 +159,19 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } } + bool is_in_turn_lane = false; + for (const auto & lane_id : unique_lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + if (turn_direction == "left" || turn_direction == "right") { + const auto & position = current_pose.position; + const lanelet::BasicPoint2d point(position.x, position.y); + if (lanelet::geometry::inside(lanelet, point)) { + is_in_turn_lane = true; + break; + } + } + } // combine consecutive lanes of the same turn direction // stores lanes that have already been combine std::set processed_lanes; @@ -251,11 +278,8 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( continue; } - constexpr double stop_velocity_threshold = 0.1; if (dist_to_front_point < search_distance) { - if ( - lane_attribute == "right" || lane_attribute == "left" || - (lane_attribute == "straight" && current_vel < stop_velocity_threshold)) { + if (requires_turn_signal(lane_attribute, is_in_turn_lane)) { // update map if necessary if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { desired_start_point_map_.emplace(lane_id, current_pose); @@ -415,15 +439,7 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const double dist_to_original_desired_start = get_distance(original_desired_start_point) - base_link2front_; - const double dist_to_original_desired_end = get_distance(original_desired_end_point); - const double dist_to_original_required_start = - get_distance(original_required_start_point) - base_link2front_; - const double dist_to_original_required_end = get_distance(original_required_end_point); const double dist_to_new_desired_start = get_distance(new_desired_start_point) - base_link2front_; - const double dist_to_new_desired_end = get_distance(new_desired_end_point); - const double dist_to_new_required_start = - get_distance(new_required_start_point) - base_link2front_; - const double dist_to_new_required_end = get_distance(new_required_end_point); // If we still do not reach the desired front point we ignore it if (dist_to_original_desired_start > 0.0 && dist_to_new_desired_start > 0.0) { @@ -435,6 +451,9 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_end = get_distance(new_desired_end_point); + // If we already passed the desired end point, return the other signal if (dist_to_original_desired_end < 0.0 && dist_to_new_desired_end < 0.0) { TurnSignalInfo empty_signal_info; @@ -445,6 +464,13 @@ TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( return original_signal; } + const double dist_to_original_required_start = + get_distance(original_required_start_point) - base_link2front_; + const double dist_to_original_required_end = get_distance(original_required_end_point); + const double dist_to_new_required_start = + get_distance(new_required_start_point) - base_link2front_; + const double dist_to_new_required_end = get_distance(new_required_end_point); + if (dist_to_original_desired_start <= dist_to_new_desired_start) { const auto enable_prior = use_prior_turn_signal( dist_to_original_required_start, dist_to_original_required_end, dist_to_new_required_start, @@ -604,4 +630,146 @@ geometry_msgs::msg::Quaternion TurnSignalDecider::calc_orientation( const double yaw = tier4_autoware_utils::calcAzimuthAngle(src_point, dst_point); return tier4_autoware_utils::createQuaternionFromRPY(0.0, pitch, yaw); } + +std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( + const ShiftedPath & path, const ShiftLine & shift_line, + const lanelet::ConstLanelets & current_lanelets, + const std::shared_ptr route_handler, + const BehaviorPathPlannerParameters & parameters, const Odometry::ConstSharedPtr self_odometry, + const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, + const bool override_ego_stopped_check, const bool is_pull_out) const +{ + const auto & p = parameters; + const auto & rh = route_handler; + const auto & ego_pose = self_odometry->pose.pose; + const auto & ego_speed = self_odometry->twist.twist.linear.x; + + if (!is_driving_forward) { + TurnSignalInfo turn_signal_info{}; + turn_signal_info.hazard_signal.command = HazardLightsCommand::ENABLE; + const auto back_start_pose = rh->getOriginalStartPose(); + const Pose & start_pose = self_odometry->pose.pose; + + turn_signal_info.desired_start_point = back_start_pose; + turn_signal_info.required_start_point = back_start_pose; + // pull_out start_pose is same to backward driving end_pose + turn_signal_info.required_end_point = start_pose; + turn_signal_info.desired_end_point = start_pose; + return std::make_pair(turn_signal_info, false); + } + + if (shift_line.start_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.start_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.end_idx + 1 > path.shift_length.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + if (shift_line.end_idx + 1 > path.path.points.size()) { + RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto [start_shift_length, end_shift_length] = + std::invoke([&path, &shift_line, &egos_lane_is_shifted]() -> std::pair { + const auto temp_start_shift_length = path.shift_length.at(shift_line.start_idx); + const auto temp_end_shift_length = path.shift_length.at(shift_line.end_idx); + // Shift is done using the target lane and not current ego's lane + if (!egos_lane_is_shifted) { + return std::make_pair(temp_end_shift_length, -temp_start_shift_length); + } + return std::make_pair(temp_start_shift_length, temp_end_shift_length); + }); + + const auto relative_shift_length = end_shift_length - start_shift_length; + + // If shift length is shorter than the threshold, it does not need to turn on blinkers + if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { + return std::make_pair(TurnSignalInfo{}, true); + } + + // If the vehicle does not shift anymore, we turn off the blinker + if ( + std::fabs(end_shift_length - current_shift_length) < + p.turn_signal_remaining_shift_length_threshold) { + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto get_command = [](const auto & shift_length) { + return shift_length > 0.0 ? TurnIndicatorsCommand::ENABLE_LEFT + : TurnIndicatorsCommand::ENABLE_RIGHT; + }; + + const auto signal_prepare_distance = + std::max(ego_speed * p.turn_signal_search_time, p.turn_signal_minimum_search_distance); + const auto ego_front_to_shift_start = + calcSignedArcLength(path.path.points, ego_pose.position, shift_line.start_idx) - + p.vehicle_info.max_longitudinal_offset_m; + + if (signal_prepare_distance < ego_front_to_shift_start) { + return std::make_pair(TurnSignalInfo{}, false); + } + + const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; + const auto blinker_end_pose = path.path.points.at(shift_line.end_idx).point.pose; + const auto get_start_pose = [&](const auto & ego_to_shift_start) { + return ego_to_shift_start > 0.0 ? ego_pose : blinker_start_pose; + }; + + TurnSignalInfo turn_signal_info{}; + turn_signal_info.desired_start_point = get_start_pose(ego_front_to_shift_start); + turn_signal_info.desired_end_point = blinker_end_pose; + turn_signal_info.required_start_point = blinker_start_pose; + turn_signal_info.required_end_point = blinker_end_pose; + turn_signal_info.turn_signal.command = get_command(relative_shift_length); + + if (!p.turn_signal_on_swerving) { + return std::make_pair(turn_signal_info, false); + } + + lanelet::ConstLanelet lanelet; + const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; + if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); + const auto left_opposite_lanes = rh->getLeftOppositeLanelets(lanelet); + const auto right_same_direction_lane = rh->getRightLanelet(lanelet, true, true); + const auto right_opposite_lanes = rh->getRightOppositeLanelets(lanelet); + const auto has_left_lane = left_same_direction_lane.has_value() || !left_opposite_lanes.empty(); + const auto has_right_lane = + right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); + + if ( + !is_pull_out && !existShiftSideLane( + start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + if (!straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { + return std::make_pair(TurnSignalInfo{}, true); + } + + constexpr double STOPPED_THRESHOLD = 0.1; // [m/s] + if (ego_speed < STOPPED_THRESHOLD && !override_ego_stopped_check) { + if (isNearEndOfShift( + start_shift_length, end_shift_length, ego_pose.position, current_lanelets, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } + } + + return std::make_pair(turn_signal_info, false); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index af4bebe183d1d..1963ab43355ea 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -35,19 +35,22 @@ template size_t findNearestSegmentIndexFromLateralDistance( const std::vector & points, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + std::optional closest_idx{std::nullopt}; double min_lateral_dist = std::numeric_limits::max(); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); - const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); const double lat_dist = [&]() { if (lon_dist < 0.0) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx), target_point); + return calcDistance2d(points.at(seg_idx), target_point); } if (segment_length < lon_dist) { - return tier4_autoware_utils::calcDistance2d(points.at(seg_idx + 1), target_point); + return calcDistance2d(points.at(seg_idx + 1), target_point); } return std::abs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); }(); @@ -64,6 +67,49 @@ size_t findNearestSegmentIndexFromLateralDistance( return motion_utils::findNearestSegmentIndex(points, target_point); } +template +size_t findNearestSegmentIndexFromLateralDistance( + const std::vector & points, const geometry_msgs::msg::Pose & target_point, + const double yaw_threshold) +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::normalizeRadian; + + std::optional closest_idx{std::nullopt}; + double min_lateral_dist = std::numeric_limits::max(); + for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { + const auto base_yaw = tf2::getYaw(target_point.orientation); + const auto yaw = + normalizeRadian(calcAzimuthAngle(points.at(seg_idx), points.at(seg_idx + 1)) - base_yaw); + if (yaw_threshold < std::abs(yaw)) { + continue; + } + const double lon_dist = + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point.position); + const double segment_length = calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); + const double lat_dist = [&]() { + if (lon_dist < 0.0) { + return calcDistance2d(points.at(seg_idx), target_point.position); + } + if (segment_length < lon_dist) { + return calcDistance2d(points.at(seg_idx + 1), target_point.position); + } + return std::abs(motion_utils::calcLateralOffset(points, target_point.position, seg_idx)); + }(); + if (lat_dist < min_lateral_dist) { + closest_idx = seg_idx; + min_lateral_dist = lat_dist; + } + } + + if (closest_idx) { + return *closest_idx; + } + + return motion_utils::findNearestSegmentIndex(points, target_point.position); +} + bool checkHasSameLane( const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelet & target_lane) { @@ -102,6 +148,41 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } + +std::vector extractBoundFromPolygon( + const lanelet::ConstPolygon3d & polygon, const size_t start_idx, const size_t end_idx, + const bool clockwise) +{ + std::vector ret{}; + + for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { + ret.push_back(polygon[i]); + + if (i + 1 == polygon.size() && clockwise) { + if (end_idx == 0) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = 0; + ret.push_back(polygon[i]); + continue; + } + + if (i == 0 && !clockwise) { + if (end_idx == polygon.size() - 1) { + ret.push_back(polygon[end_idx]); + return ret; + } + i = polygon.size() - 1; + ret.push_back(polygon[i]); + continue; + } + } + + ret.push_back(polygon[end_idx]); + + return ret; +} } // namespace namespace behavior_path_planner::utils::drivable_area_processing @@ -128,26 +209,25 @@ std::optional> intersectBound( return std::nullopt; } -double calcDistanceFromPointToSegment( +double calcSquaredDistanceFromPointToSegment( const geometry_msgs::msg::Point & segment_start_point, const geometry_msgs::msg::Point & segment_end_point, const geometry_msgs::msg::Point & target_point) { + using tier4_autoware_utils::calcSquaredDistance2d; + const auto & a = segment_start_point; const auto & b = segment_end_point; const auto & p = target_point; const double dot_val = (b.x - a.x) * (p.x - a.x) + (b.y - a.y) * (p.y - a.y); - const double squared_segment_length = tier4_autoware_utils::calcSquaredDistance2d(a, b); + const double squared_segment_length = calcSquaredDistance2d(a, b); if (0 <= dot_val && dot_val <= squared_segment_length) { - const double numerator = std::abs((p.x - a.x) * (a.y - b.y) - (p.y - a.y) * (a.x - b.x)); - const double denominator = std::sqrt(std::pow(a.x - b.x, 2) + std::pow(a.y - b.y, 2)); - return numerator / denominator; + return calcSquaredDistance2d(p, a) - dot_val * dot_val / squared_segment_length; } // target_point is outside the segment. - return std::min( - tier4_autoware_utils::calcDistance2d(a, p), tier4_autoware_utils::calcDistance2d(b, p)); + return std::min(calcSquaredDistance2d(a, p), calcSquaredDistance2d(b, p)); } PolygonPoint transformBoundFrenetCoordinate( @@ -158,8 +238,8 @@ PolygonPoint transformBoundFrenetCoordinate( // find wrong nearest index. std::vector dist_to_bound_segment_vec; for (size_t i = 0; i < bound_points.size() - 1; ++i) { - const double dist_to_bound_segment = - calcDistanceFromPointToSegment(bound_points.at(i), bound_points.at(i + 1), target_point); + const double dist_to_bound_segment = calcSquaredDistanceFromPointToSegment( + bound_points.at(i), bound_points.at(i + 1), target_point); dist_to_bound_segment_vec.push_back(dist_to_bound_segment); } @@ -690,54 +770,27 @@ std::vector getNonOverlappingExpandedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const double vehicle_length, const std::shared_ptr planner_data, - const bool is_driving_forward) + const bool enable_expanding_freespace_areas, + const std::shared_ptr planner_data, const bool is_driving_forward) { - // extract data - const auto transformed_lanes = utils::transformToLanelets(lanes); - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto route_handler = planner_data->route_handler; - constexpr double overlap_threshold = 0.01; - if (path.points.empty()) { return; } - auto addPoints = - [](const lanelet::ConstLineString3d & points, std::vector & bound) { - for (const auto & bound_p : points) { - const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if (bound.empty()) { - bound.push_back(cp); - } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { - bound.push_back(cp); - } - } - }; - - const auto has_overlap = - [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { - for (const auto & transformed_lane : transformed_lanes) { - if (checkHasSameLane(ignore_lanelets, transformed_lane)) { - continue; - } - if (boost::geometry::intersects( - lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; + path.left_bound.clear(); + path.right_bound.clear(); // Insert Position - auto left_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, true); - auto right_bound = calcBound( - route_handler, lanes, enable_expanding_hatched_road_markings, - enable_expanding_intersection_areas, false); - - if (left_bound.empty() || right_bound.empty()) { + path.left_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, true, + is_driving_forward); + path.right_bound = calcBound( + path, planner_data, lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, enable_expanding_freespace_areas, false, + is_driving_forward); + + if (path.left_bound.empty() || path.right_bound.empty()) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("behavior_path_planner").get_child("utils"), clock, 1000, @@ -745,135 +798,10 @@ void generateDrivableArea( return; } - // Insert points after goal - lanelet::ConstLanelet goal_lanelet; - if ( - route_handler->getGoalLanelet(&goal_lanelet) && - checkHasSameLane(transformed_lanes, goal_lanelet)) { - const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); - const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); - const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); - const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); - lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; - if (goal_left_lanelet) { - goal_lanelets.push_back(*goal_left_lanelet); - } - if (goal_right_lanelet) { - goal_lanelets.push_back(*goal_right_lanelet); - } - - for (const auto & lane : lanes_after_goal) { - // If lane is already in the transformed lanes, ignore it - if (checkHasSameLane(transformed_lanes, lane)) { - continue; - } - // Check if overlapped - const bool is_overlapped = - (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) - : has_overlap(lane)); - if (is_overlapped) { - continue; - } - - addPoints(lane.leftBound3d(), left_bound); - addPoints(lane.rightBound3d(), right_bound); - } - } - - if (!is_driving_forward) { - std::reverse(left_bound.begin(), left_bound.end()); - std::reverse(right_bound.begin(), right_bound.end()); - } - - path.left_bound.clear(); - path.right_bound.clear(); - - const auto [left_start_idx, right_start_idx] = [&]() { - const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); - const auto cropped_path_points = motion_utils::cropPoints( - path.points, current_pose.position, current_seg_idx, - planner_data->parameters.forward_path_length, - planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); - - constexpr double front_length = 0.5; - const auto front_pose = - cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; - const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); - const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); - const auto left_start_point = - calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_point = calcLongitudinalOffsetStartPoint( - right_bound, front_pose, front_right_start_idx, -front_length); - const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); - const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); - - // Insert a start point - path.left_bound.push_back(left_start_point); - path.right_bound.push_back(right_start_point); - - return std::make_pair(left_start_idx, right_start_idx); - }(); - - // Get Closest segment for the goal point - const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; - const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); - const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position); - const auto left_goal_point = - calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_point = - calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); - const size_t left_goal_idx = std::max( - goal_left_start_idx, findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point)); - const size_t right_goal_idx = std::max( - goal_right_start_idx, - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point)); - - // Insert middle points - for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); - if (dist > overlap_threshold) { - path.left_bound.push_back(next_point); - } - } - for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i); - const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); - if (dist > overlap_threshold) { - path.right_bound.push_back(next_point); - } - } - - // Insert a goal point - if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > - overlap_threshold) { - path.left_bound.push_back(left_goal_point); - } - if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > - overlap_threshold) { - path.right_bound.push_back(right_goal_point); - } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { drivable_area_expansion::expand_drivable_area(path, planner_data); } - - // make bound longitudinally monotonic - // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if ( - is_driving_forward && - (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) { - makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound - makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound - } } void generateDrivableArea( @@ -1243,36 +1171,6 @@ std::vector getBoundWithIntersectionAreas( const std::shared_ptr & route_handler, const std::vector & drivable_lanes, const bool is_left) { - const auto extract_bound_from_polygon = - [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret{}; - for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(polygon[i]); - - if (i + 1 == polygon.size() && clockwise) { - if (end_idx == 0) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = 0; - continue; - } - - if (i == 0 && !clockwise) { - if (end_idx == polygon.size() - 1) { - ret.push_back(polygon[end_idx]); - return ret; - } - i = polygon.size() - 1; - continue; - } - } - - ret.push_back(polygon[end_idx]); - - return ret; - }; - std::vector expanded_bound = original_bound; // expand drivable area by using intersection area. @@ -1318,7 +1216,7 @@ std::vector getBoundWithIntersectionAreas( const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); intersection_bound = - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); + extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); } // Step2. check shared bound point. @@ -1379,15 +1277,333 @@ std::vector getBoundWithIntersectionAreas( return expanded_bound; } +std::pair, bool> getBoundWithFreeSpaceAreas( + const std::vector & original_bound, + const std::vector & other_side_bound, + const std::shared_ptr planner_data, const bool is_left) +{ + using lanelet::utils::to2D; + using lanelet::utils::conversion::toGeomMsgPt; + using lanelet::utils::conversion::toLaneletPoint; + using tier4_autoware_utils::getPose; + using tier4_autoware_utils::pose2transform; + using tier4_autoware_utils::transformVector; + + const auto & route_handler = planner_data->route_handler; + const auto & ego_pose = planner_data->self_odometry->pose.pose; + + auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr()); + if (polygons.empty()) { + return std::make_pair(original_bound, false); + } + + std::sort(polygons.begin(), polygons.end(), [&ego_pose](const auto & a, const auto & b) { + const double a_distance = boost::geometry::distance( + to2D(a).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + const double b_distance = boost::geometry::distance( + to2D(b).basicPolygon(), to2D(toLaneletPoint(ego_pose.position)).basicPoint()); + return a_distance < b_distance; + }); + + const auto & polygon = polygons.front(); + + const auto original_bound_itr = + std::find_if(original_bound.begin(), original_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto original_bound_rev_itr = + std::find_if(original_bound.rbegin(), original_bound.rend(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + const auto other_side_bound_itr = + std::find_if(other_side_bound.begin(), other_side_bound.end(), [&polygon](const auto & p) { + return std::any_of(polygon.begin(), polygon.end(), [&p](const auto & p_outer) { + return p.id() == p_outer.id(); + }); + }); + + if ( + original_bound_itr == original_bound.end() || other_side_bound_itr == other_side_bound.end()) { + return std::make_pair(original_bound, false); + } + + const auto footprint = planner_data->parameters.vehicle_info.createFootprint(); + const auto vehicle_polygon = transformVector(footprint, pose2transform(ego_pose)); + const auto is_driving_freespace = + !boost::geometry::disjoint(vehicle_polygon, to2D(polygon).basicPolygon()); + + const auto is_clockwise_polygon = boost::geometry::is_valid(to2D(polygon.basicPolygon())); + const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; + + const auto extract_bound_from_polygon = [&polygon, &is_clockwise_iteration]( + const auto & start_id, const auto & end_id) { + const auto start_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == start_id; }); + + const auto end_point_itr = std::find_if( + polygon.begin(), polygon.end(), [&](const auto & p) { return p.id() == end_id; }); + + // extract line strings between start_idx and end_idx. + const size_t start_idx = std::distance(polygon.begin(), start_point_itr); + const size_t end_idx = std::distance(polygon.begin(), end_point_itr); + + return extractBoundFromPolygon(polygon, start_idx, end_idx, is_clockwise_iteration); + }; + + const auto get_bound_edge = [&ego_pose, &is_driving_freespace, &is_left]( + const auto & bound, const auto trim_behind_bound) { + if (!is_driving_freespace) { + return bound; + } + + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + ego_pose, (trim_behind_bound ? -100.0 : 100.0), (is_left ? 0.1 : -0.1), 0.0); + + std::vector ret; + for (size_t i = 1; i < bound.size(); ++i) { + const auto intersect = tier4_autoware_utils::intersect( + ego_pose.position, p_offset.position, toGeomMsgPt(bound.at(i - 1)), + toGeomMsgPt(bound.at(i))); + + ret.push_back(bound.at(i - 1)); + + if (intersect.has_value()) { + ret.emplace_back( + lanelet::InvalId, intersect.value().x, intersect.value().y, intersect.value().z); + break; + } + } + + return ret; + }; + + std::vector expanded_bound; + + enum class RouteCase { + ROUTE_IS_PASS_THROUGH_FREESPACE = 0, + GOAL_POS_IS_IN_FREESPACE, + INIT_POS_IS_IN_FREESPACE, + OTHER, + }; + + const auto route_case = [&]() { + if (original_bound_itr->id() != original_bound_rev_itr->id()) { + return RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.front().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::INIT_POS_IS_IN_FREESPACE; + } else if (boost::geometry::within( + to2D(original_bound.back().basicPoint()), to2D(polygon).basicPolygon())) { + return RouteCase::GOAL_POS_IS_IN_FREESPACE; + } + return RouteCase::OTHER; + }(); + + switch (route_case) { + case RouteCase::ROUTE_IS_PASS_THROUGH_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), original_bound_rev_itr->id()); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), polygon_bound.begin(), polygon_bound.end()); + expanded_bound.insert( + expanded_bound.end(), std::next(original_bound_rev_itr).base(), original_bound.end()); + break; + } + case RouteCase::INIT_POS_IS_IN_FREESPACE: { + auto polygon_bound = + extract_bound_from_polygon(other_side_bound_itr->id(), original_bound_itr->id()); + std::reverse(polygon_bound.begin(), polygon_bound.end()); + auto bound_edge = get_bound_edge(polygon_bound, true); + std::reverse(bound_edge.begin(), bound_edge.end()); + + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + expanded_bound.insert(expanded_bound.end(), original_bound_itr, original_bound.end()); + break; + } + case RouteCase::GOAL_POS_IS_IN_FREESPACE: { + const auto polygon_bound = + extract_bound_from_polygon(original_bound_itr->id(), other_side_bound_itr->id()); + const auto bound_edge = get_bound_edge(polygon_bound, false); + + expanded_bound.insert(expanded_bound.end(), original_bound.begin(), original_bound_itr); + expanded_bound.insert(expanded_bound.end(), bound_edge.begin(), bound_edge.end()); + break; + } + case RouteCase::OTHER: { + expanded_bound = original_bound; + break; + } + default: + throw std::domain_error("invalid case."); + } + + const auto goal_is_in_freespace = boost::geometry::within( + to2D(toLaneletPoint(route_handler->getGoalPose().position).basicPoint()), + to2D(polygon).basicPolygon()); + + return std::make_pair(expanded_bound, is_driving_freespace || goal_is_in_freespace); +} + +std::vector postProcess( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr planner_data, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left, const bool is_driving_forward) +{ + const auto lanelets = utils::transformToLanelets(drivable_lanes); + const auto & current_pose = planner_data->self_odometry->pose.pose; + const auto & route_handler = planner_data->route_handler; + const auto & vehicle_length = planner_data->parameters.vehicle_length; + constexpr double overlap_threshold = 0.01; + + const auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } + } + }; + + const auto has_overlap = + [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { + for (const auto & transformed_lane : lanelets) { + if (checkHasSameLane(ignore_lanelets, transformed_lane)) { + continue; + } + if (boost::geometry::intersects( + lane.polygon2d().basicPolygon(), transformed_lane.polygon2d().basicPolygon())) { + return true; + } + } + return false; + }; + + std::vector processed_bound; + std::vector tmp_bound = original_bound; + + // Insert points after goal + lanelet::ConstLanelet goal_lanelet; + if (route_handler->getGoalLanelet(&goal_lanelet) && checkHasSameLane(lanelets, goal_lanelet)) { + const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); + const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); + const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); + lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; + if (goal_left_lanelet.has_value()) { + goal_lanelets.push_back(goal_left_lanelet.value()); + } + if (goal_right_lanelet.has_value()) { + goal_lanelets.push_back(goal_right_lanelet.value()); + } + + for (const auto & lane : lanes_after_goal) { + // If lane is already in the transformed lanes, ignore it + if (checkHasSameLane(lanelets, lane)) { + continue; + } + // Check if overlapped + const bool is_overlapped = + (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) + : has_overlap(lane)); + if (is_overlapped) { + continue; + } + + if (is_left) { + addPoints(lane.leftBound3d(), tmp_bound); + } else { + addPoints(lane.rightBound3d(), tmp_bound); + } + } + } + + if (!is_driving_forward) { + std::reverse(tmp_bound.begin(), tmp_bound.end()); + } + + const auto start_idx = [&]() { + const size_t current_seg_idx = planner_data->findEgoSegmentIndex(path.points); + const auto cropped_path_points = motion_utils::cropPoints( + path.points, current_pose.position, current_seg_idx, + planner_data->parameters.forward_path_length, + planner_data->parameters.backward_path_length + planner_data->parameters.input_path_interval); + + constexpr double front_length = 0.5; + const auto front_pose = + cropped_path_points.empty() ? current_pose : cropped_path_points.front().point.pose; + const size_t front_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, front_pose, M_PI_2); + const auto start_point = + calcLongitudinalOffsetStartPoint(tmp_bound, front_pose, front_start_idx, -front_length); + + // Insert a start point + processed_bound.push_back(start_point); + + return findNearestSegmentIndexFromLateralDistance(tmp_bound, start_point); + }(); + + // Get Closest segment for the goal point + const auto [goal_idx, goal_point] = [&]() { + const auto goal_pose = path.points.empty() ? current_pose : path.points.back().point.pose; + const size_t goal_start_idx = + findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_pose, M_PI_2); + const auto goal_point = + calcLongitudinalOffsetGoalPoint(tmp_bound, goal_pose, goal_start_idx, vehicle_length); + const size_t goal_idx = + std::max(goal_start_idx, findNearestSegmentIndexFromLateralDistance(tmp_bound, goal_point)); + + return std::make_pair(goal_idx, goal_point); + }(); + + // Insert middle points + for (size_t i = start_idx + 1; i <= goal_idx; ++i) { + const auto & next_point = tmp_bound.at(i); + const double dist = tier4_autoware_utils::calcDistance2d(processed_bound.back(), next_point); + if (dist > overlap_threshold) { + processed_bound.push_back(next_point); + } + } + + // Insert a goal point + if ( + tier4_autoware_utils::calcDistance2d(processed_bound.back(), goal_point) > overlap_threshold) { + processed_bound.push_back(goal_point); + } + + const bool skip_monotonic_process = + !is_driving_forward || + !(enable_expanding_hatched_road_markings || enable_expanding_intersection_areas); + + return skip_monotonic_process + ? processed_bound + : makeBoundLongitudinallyMonotonic(processed_bound, path, planner_data, is_left); +} + // calculate bounds from drivable lanes and hatched road markings std::vector calcBound( - const std::shared_ptr route_handler, + const PathWithLaneId & path, const std::shared_ptr planner_data, const std::vector & drivable_lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) + const bool enable_expanding_freespace_areas, const bool is_left, const bool is_driving_forward) { + using motion_utils::removeOverlapPoints; + + const auto & route_handler = planner_data->route_handler; + // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { + const auto convert_to_points = [](const auto & drivable_lanes, const auto is_left) { constexpr double overlap_threshold = 0.01; std::vector points; @@ -1414,20 +1630,35 @@ std::vector calcBound( }; // Step1. create drivable bound from drivable lanes. - std::vector bound_points = convert_to_points(drivable_lanes); + auto [bound_points, skip_post_process] = [&]() { + if (!enable_expanding_freespace_areas) { + return std::make_pair(convert_to_points(drivable_lanes, is_left), false); + } + return getBoundWithFreeSpaceAreas( + convert_to_points(drivable_lanes, is_left), convert_to_points(drivable_lanes, !is_left), + planner_data, is_left); + }(); + + const auto post_process = [&](const auto & bound, const auto skip) { + return skip + ? bound + : postProcess( + bound, path, planner_data, drivable_lanes, enable_expanding_hatched_road_markings, + enable_expanding_intersection_areas, is_left, is_driving_forward); + }; // Step2. if there is no drivable area defined by polygon, return original drivable bound. if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } - // Step3. if there are hatched road markings, expand drivable bound with the polygon. + // Step3.if there are hatched road markings, expand drivable bound with the polygon. if (enable_expanding_hatched_road_markings) { bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); } if (!enable_expanding_intersection_areas) { - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } // Step4. if there are intersection areas, expand drivable bound with the polygon. @@ -1436,12 +1667,12 @@ std::vector calcBound( getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); } - return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + return post_process(removeOverlapPoints(to_ros_point(bound_points)), skip_post_process); } -void makeBoundLongitudinallyMonotonic( - PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_bound_left) +std::vector makeBoundLongitudinallyMonotonic( + const std::vector & original_bound, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const bool is_left) { using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcAzimuthAngle; @@ -1487,7 +1718,7 @@ void makeBoundLongitudinallyMonotonic( const auto get_bound_with_pose = [&](const auto & bound_with_pose, const auto & path_points) { auto ret = bound_with_pose; - const double offset = is_bound_left ? 100.0 : -100.0; + const double offset = is_left ? 100.0 : -100.0; size_t start_bound_idx = 0; @@ -1567,14 +1798,14 @@ void makeBoundLongitudinallyMonotonic( // NOTE: is_bound_left is used instead of is_points_left since orientation of path point is // opposite. - const double lat_offset = is_bound_left ? 100.0 : -100.0; + const double lat_offset = is_left ? 100.0 : -100.0; 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(p_bound_1, 0.0, lat_offset, 0.0); - if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_bound_left, is_reverse)) { + if (!is_non_monotonic(p_bound_1, p_bound_2.position, is_left, is_reverse)) { bound_idx++; continue; } @@ -1660,18 +1891,16 @@ void makeBoundLongitudinallyMonotonic( return ret; }; - const auto original_bound = is_bound_left ? path.left_bound : path.right_bound; - if (path.points.empty()) { - return; + return original_bound; } - if (original_bound.empty()) { - return; + if (original_bound.size() < 2) { + return original_bound; } if (path.points.front().lane_ids.empty()) { - return; + return original_bound; } // step.1 create bound with pose vector. @@ -1713,14 +1942,14 @@ void makeBoundLongitudinallyMonotonic( p.forward_path_length, p); if (centerline_path.points.size() < 2) { - return; + return original_bound; } const auto ego_idx = planner_data->findEgoIndex(centerline_path.points); const auto end_idx = findNearestSegmentIndex(centerline_path.points, original_bound.back()); if (ego_idx >= end_idx) { - return; + return original_bound; } clipped_points.insert( @@ -1729,7 +1958,7 @@ void makeBoundLongitudinallyMonotonic( } if (clipped_points.empty()) { - return; + return original_bound; } // step.3 update bound pose by reference path pose. @@ -1750,11 +1979,7 @@ void makeBoundLongitudinallyMonotonic( auto full_monotonic_bound = remove_orientation(full_monotonic_bound_with_pose); // step.7 remove sharp bound points. - if (is_bound_left) { - path.left_bound = remove_sharp_points(full_monotonic_bound); - } else { - path.right_bound = remove_sharp_points(full_monotonic_bound); - } + return remove_sharp_points(full_monotonic_bound); } std::vector combineDrivableLanes( @@ -1764,7 +1989,9 @@ std::vector combineDrivableLanes( const auto has_same_lane = [](const lanelet::ConstLanelet & target_lane, const lanelet::ConstLanelets & lanes) { return std::find_if(lanes.begin(), lanes.end(), [&](const auto & ll) { - return ll.id() == target_lane.id(); + return ll.id() == target_lane.id() && + ll.rightBound3d().id() == target_lane.rightBound3d().id() && + ll.leftBound3d().id() == target_lane.leftBound3d().id(); }) != lanes.end(); }; @@ -1863,6 +2090,15 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // enable expanding freespace areas + combined_drivable_area_info.enable_expanding_freespace_areas = + drivable_area_info1.enable_expanding_freespace_areas || + drivable_area_info2.enable_expanding_freespace_areas; + + // drivable margin + combined_drivable_area_info.drivable_margin = + std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); + return combined_drivable_area_info; } diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 7ba4f114328a7..8a89aa139ae44 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -132,7 +132,7 @@ std::vector GeometricParallelParking::generatePullOverPaths( : parameters_.backward_parking_path_interval; auto arc_paths = planOneTrial( start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, - end_pose_offset, lane_departure_margin, arc_path_interval); + end_pose_offset, lane_departure_margin, arc_path_interval, {}); if (arc_paths.empty()) { return std::vector{}; } @@ -238,7 +238,8 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start) + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, + const std::shared_ptr lane_departure_checker) { constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose @@ -258,7 +259,7 @@ bool GeometricParallelParking::planPullOut( auto arc_paths = planOneTrial( *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start, start_pose_offset, parameters_.pull_out_lane_departure_margin, - parameters_.pull_out_path_interval); + parameters_.pull_out_path_interval, lane_departure_checker); if (arc_paths.empty()) { // not found path continue; @@ -378,7 +379,8 @@ std::vector GeometricParallelParking::planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, - const double lane_departure_margin, const double arc_path_interval) + const double lane_departure_margin, const double arc_path_interval, + const std::shared_ptr lane_departure_checker) { clearPaths(); @@ -512,6 +514,24 @@ std::vector GeometricParallelParking::planOneTrial( setLaneIdsToPath(path_turn_first); setLaneIdsToPath(path_turn_second); + if (lane_departure_checker) { + const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + const bool is_path_turn_first_outside_lanes = + lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_first); + + if (is_path_turn_first_outside_lanes) { + return std::vector{}; + } + + const bool is_path_turn_second_outside_lanes = + lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_second); + + if (is_path_turn_second_outside_lanes) { + return std::vector{}; + } + } + // generate arc path vector paths_.push_back(path_turn_first); paths_.push_back(path_turn_second); diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 188c50993e4ae..c2f9a8796c483 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -330,8 +330,9 @@ TargetObjectsOnLane createTargetObjectsOnLane( lanelet::ConstLanelets all_left_lanelets; lanelet::ConstLanelets all_right_lanelets; + // TODO(sugahara): consider right side parking and define right shoulder lanelets + lanelet::ConstLanelets all_left_shoulder_lanelets; - // Define lambda functions to update left and right lanelets const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { const auto left_lanelets = route_handler->getAllLeftSharedLinestringLanelets( target_lane, include_opposite, invert_opposite); @@ -345,13 +346,23 @@ TargetObjectsOnLane createTargetObjectsOnLane( all_right_lanelets.end(), right_lanelets.begin(), right_lanelets.end()); }; - // Update left and right lanelets for each current lane + const auto update_left_shoulder_lanelet = [&](const lanelet::ConstLanelet & target_lane) { + lanelet::ConstLanelet neighbor_shoulder_lane{}; + const bool shoulder_lane_is_found = + route_handler->getLeftShoulderLanelet(target_lane, &neighbor_shoulder_lane); + if (shoulder_lane_is_found) { + all_left_shoulder_lanelets.insert(all_left_shoulder_lanelets.end(), neighbor_shoulder_lane); + } + }; + for (const auto & current_lane : current_lanes) { update_left_lanelets(current_lane); update_right_lanelets(current_lane); + update_left_shoulder_lanelet(current_lane); } TargetObjectsOnLane target_objects_on_lane{}; + const auto append_objects_on_lane = [&](auto & lane_objects, const auto & check_lanes) { std::for_each( filtered_objects.objects.begin(), filtered_objects.objects.end(), [&](const auto & object) { @@ -362,7 +373,7 @@ TargetObjectsOnLane createTargetObjectsOnLane( }); }; - // TODO(Sugahara): Consider shoulder and other lane objects + // TODO(Sugahara): Consider other lane objects if (object_lane_configuration.check_current_lane && !current_lanes.empty()) { append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes); } @@ -372,6 +383,9 @@ TargetObjectsOnLane createTargetObjectsOnLane( if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) { append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets); } + if (object_lane_configuration.check_shoulder_lane && !all_left_shoulder_lanelets.empty()) { + append_objects_on_lane(target_objects_on_lane.on_shoulder_lane, all_left_shoulder_lanelets); + } return target_objects_on_lane; } diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index f5d76f5cd4d35..4b0697dbe9605 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -184,6 +184,88 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +Polygon2d createExtendedPolygonAlongPath( + const PathWithLaneId & planned_path, const Pose & base_link_pose, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length, + const double lat_margin, const double is_stopped_obj, CollisionCheckDebug & debug) +{ + const double & base_to_front = vehicle_info.max_longitudinal_offset_m; + const double & width = vehicle_info.vehicle_width_m; + const double & base_to_rear = vehicle_info.rear_overhang_m; + + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = + -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value + const double lat_offset = width / 2.0 + lat_margin; + + { + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = lat_offset; + } + + const auto lon_offset_pose = motion_utils::calcLongitudinalOffsetPose( + planned_path.points, base_link_pose.position, lon_length); + if (!lon_offset_pose.has_value()) { + return createExtendedPolygon( + base_link_pose, vehicle_info, lon_length, lat_margin, is_stopped_obj, debug); + } + + const size_t start_idx = + motion_utils::findNearestSegmentIndex(planned_path.points, base_link_pose.position); + const size_t end_idx = + motion_utils::findNearestSegmentIndex(planned_path.points, lon_offset_pose.value().position); + + Polygon2d polygon; + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + for (size_t i = start_idx + 1; i < end_idx + 1; ++i) { + const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); + const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(lon_offset_pose.value(), base_to_front, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = tier4_autoware_utils::calcOffsetPose( + lon_offset_pose.value(), base_to_front, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + for (size_t i = end_idx; i > start_idx; --i) { + const auto p = tier4_autoware_utils::getPose(planned_path.points.at(i)); + const auto p_offset = tier4_autoware_utils::calcOffsetPose(p, 0.0, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + { + const auto p_offset = + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); + appendPointToPolygon(polygon, p_offset.position); + } + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector createExtendedPolygonsFromPoseWithVelocityStamped( const std::vector & predicted_path, const VehicleInfo & vehicle_info, const double forward_margin, const double backward_margin, const double lat_margin) @@ -552,10 +634,24 @@ std::vector getCollidedPolygons( const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; // TODO(watanabe) fix hard coding value const bool is_stopped_object = object_velocity < 0.3; - const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon( - ego_pose, ego_vehicle_info, lon_offset, - lat_margin, is_stopped_object, debug) - : ego_polygon; + const auto extended_ego_polygon = [&]() { + if (!is_object_front) { + return ego_polygon; + } + + if (rss_parameters.extended_polygon_policy == "rectangle") { + return createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, debug); + } + + if (rss_parameters.extended_polygon_policy == "along_path") { + return createExtendedPolygonAlongPath( + planned_path, ego_pose, ego_vehicle_info, lon_offset, lat_margin, is_stopped_object, + debug); + } + + throw std::domain_error("invalid rss parameter. please select 'rectangle' or 'along_path'."); + }(); const auto & extended_obj_polygon = is_object_front ? obj_polygon diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 850919f539e59..9deaf6c927d69 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -177,4 +177,23 @@ bool isStoppedAtRedTrafficLightWithinDistance( return (distance_to_red_traffic_light < distance_threshold); } +bool isTrafficSignalStop( + const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + return true; + } + } + } + + return false; +} + } // namespace behavior_path_planner::utils::traffic_light diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 7c3a5883989fb..a528b2232520a 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1450,6 +1450,70 @@ lanelet::ConstLanelets getExtendedCurrentLanes( return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); } +lanelet::ConstLanelets getExtendedCurrentLanesFromPath( + const PathWithLaneId & path, const std::shared_ptr & planner_data, + const double backward_length, const double forward_length, const bool forward_only_in_route) +{ + auto lanes = getCurrentLanesFromPath(path, planner_data); + + if (lanes.empty()) return lanes; + const auto start_lane = lanes.front(); + double forward_length_sum = 0.0; + double backward_length_sum = 0.0; + + while (backward_length_sum < backward_length) { + auto extended_lanes = extendPrevLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for backward_length, + // the extending process will not finish. + if (extended_lanes.front().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + backward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.front()); + } else { + break; // no more previous lanes to add + } + lanes = extended_lanes; + } + + while (forward_length_sum < forward_length) { + auto extended_lanes = extendNextLane(planner_data->route_handler, lanes); + if (extended_lanes.empty()) { + return lanes; + } + // loop check + // if current map lanes is looping and has a very large value for forward_length, + // the extending process will not finish. + if (extended_lanes.back().id() == start_lane.id()) { + return lanes; + } + + if (extended_lanes.size() > lanes.size()) { + forward_length_sum += lanelet::utils::getLaneletLength2d(extended_lanes.back()); + } else { + break; // no more next lanes to add + } + + // stop extending when the lane outside of the route is reached + // if forward_length is a very large value, set it to true, + // as it may continue to extend forever. + if (forward_only_in_route) { + if (!planner_data->route_handler->isRouteLanelet(extended_lanes.back())) { + return lanes; + } + } + + lanes = extended_lanes; + } + + return lanes; +} + lanelet::ConstLanelets calcLaneAroundPose( const std::shared_ptr route_handler, const Pose & pose, const double forward_length, const double backward_length, const double dist_threshold, const double yaw_threshold) @@ -1551,4 +1615,22 @@ std::string convertToSnakeCase(const std::string & input_str) } return output_str; } + +bool isAllowedGoalModification(const std::shared_ptr & route_handler) +{ + return route_handler->isAllowedGoalModification() || checkOriginalGoalIsInShoulder(route_handler); +} + +bool checkOriginalGoalIsInShoulder(const std::shared_ptr & route_handler) +{ + const Pose & goal_pose = route_handler->getOriginalGoalPose(); + const auto shoulder_lanes = route_handler->getShoulderLanelets(); + + lanelet::ConstLanelet closest_shoulder_lane{}; + if (lanelet::utils::query::getClosestLanelet(shoulder_lanes, goal_pose, &closest_shoulder_lane)) { + return lanelet::utils::isInLanelet(goal_pose, closest_shoulder_lane, 0.1); + } + + return false; +} } // namespace behavior_path_planner::utils diff --git a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index 94be083bdebcd..eb7d1afe27549 100644 --- a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml index 514d61e225ecd..9c999b1e1b6a0 100644 --- a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml +++ b/planning/behavior_path_start_planner_module/config/start_planner.param.yaml @@ -36,6 +36,7 @@ backward_path_update_duration: 3.0 ignore_distance_from_lane_end: 15.0 # turns signal + prepare_time_before_start: 0.0 th_turn_signal_on_lateral_offset: 1.0 intersection_search_length: 30.0 length_ratio_for_turn_signal_deactivation_near_intersection: 0.5 @@ -129,7 +130,7 @@ rss_params: rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 + lateral_distance_max_threshold: 3.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 164868b47535e..68a5d8100a271 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -19,20 +19,27 @@ #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include + #include +#include + namespace behavior_path_planner { class GeometricPullOut : public PullOutPlannerBase { public: - explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); + explicit GeometricPullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const std::shared_ptr lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; + std::shared_ptr lane_departure_checker_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index a143f34ead649..a45afe560968b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -33,6 +33,7 @@ #include +#include #include #include @@ -69,6 +70,10 @@ struct PullOutStatus bool prev_is_safe_dynamic_objects{false}; std::shared_ptr prev_stop_path_after_approval{nullptr}; bool has_stop_point{false}; + std::optional stop_pose{std::nullopt}; + //! record the first time when ego started forward-driving (maybe after backward driving + //! completion) in AUTONOMOUS operation mode + std::optional first_engaged_and_driving_forward_time{std::nullopt}; PullOutStatus() {} }; @@ -139,7 +144,7 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override; + bool canTransitIdleToRunningState() override { return true; } /** * @brief init member variables. @@ -192,6 +197,10 @@ class StartPlannerModule : public SceneModuleInterface PullOutStatus status_; mutable StartGoalPlannerData start_planner_data_; + // Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this + // module's output. If the ego vehicle is in this lanelet, the calculation is skipped. + std::optional ignore_signal_{std::nullopt}; + std::deque odometry_buffer_; std::unique_ptr last_route_received_time_; @@ -214,7 +223,7 @@ class StartPlannerModule : public SceneModuleInterface std::shared_ptr lane_departure_checker_; // turn signal - TurnSignalInfo calcTurnSignalInfo() const; + TurnSignalInfo calcTurnSignalInfo(); void incrementPathIndex(); PathWithLaneId getCurrentPath() const; @@ -228,6 +237,7 @@ class StartPlannerModule : public SceneModuleInterface void updateStatusAfterBackwardDriving(); PredictedObjects filterStopObjectsInPullOutLanes( const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; + bool needToPrepareBlinkerBeforeStartDrivingForward() const; bool hasFinishedPullOut() const; bool hasFinishedBackwardDriving() const; bool hasCollisionWithDynamicObjects() const; @@ -239,6 +249,7 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + lanelet::ConstLanelets createDepartureCheckLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp index 783aace0983ca..c50648ecd3731 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp @@ -38,6 +38,7 @@ struct StartPlannerParameters double th_arrived_distance{0.0}; double th_stopped_velocity{0.0}; double th_stopped_time{0.0}; + double prepare_time_before_start{0.0}; double th_turn_signal_on_lateral_offset{0.0}; double th_distance_to_middle_of_the_road{0.0}; double intersection_search_length{0.0}; diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index 7d6dd60398e8e..3ff7e998cdff7 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -30,9 +30,12 @@ namespace behavior_path_planner { using start_planner_utils::getPullOutLanes; -GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) +GeometricPullOut::GeometricPullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const std::shared_ptr lane_departure_checker) : PullOutPlannerBase{node, parameters}, - parallel_parking_parameters_{parameters.parallel_parking_parameters} + parallel_parking_parameters_{parameters.parallel_parking_parameters}, + lane_departure_checker_(lane_departure_checker) { planner_.setParameters(parallel_parking_parameters_); } @@ -55,8 +58,8 @@ std::optional GeometricPullOut::plan(const Pose & start_pose, const planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); planner_.setPlannerData(planner_data_); - const bool found_valid_path = - planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start); + const bool found_valid_path = planner_.planPullOut( + start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index 58d0fbee7921b..c8c12363921b1 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -36,6 +36,7 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) 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"); + p.prepare_time_before_start = node->declare_parameter(ns + "prepare_time_before_start"); p.th_turn_signal_on_lateral_offset = node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); p.th_distance_to_middle_of_the_road = diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 73952ca02f558..fe83ca45a8185 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -73,11 +73,10 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // get safe path for (auto & pull_out_path : pull_out_paths) { - auto & shift_path = - pull_out_path.partial_paths.front(); // shift path is not separate but only one. - - // check lane_departure and collision with path between pull_out_start to pull_out_end - PathWithLaneId path_start_to_end{}; + // shift path is not separate but only one. + auto & shift_path = pull_out_path.partial_paths.front(); + // check lane_departure with path between pull_out_start to pull_out_end + PathWithLaneId path_shift_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); @@ -93,63 +92,35 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return shift_path.points.size() - 1; } }); - path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, shift_path.points.begin() + collision_check_end_idx + 1); } - // extract shoulder lanes from pull out lanes - lanelet::ConstLanelets shoulder_lanes; - std::copy_if( - pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), - [&route_handler](const auto & pull_out_lane) { - return route_handler->isShoulderLanelet(pull_out_lane); - }); - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip)); + // check lane departure + // The method for lane departure checking verifies if the footprint of each point on the path is + // contained within a lanelet using `boost::geometry::within`, which incurs a high computational + // cost. + const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + if ( + parameters_.check_shift_path_lane_departure && + lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) { + continue; + } // crop backward path // removes points which are out of lanes up to the start pose. // this ensures that the backward_path stays within the drivable area when starting from a // narrow place. + const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); - PathWithLaneId cropped_path{}; - for (size_t i = 0; i < shift_path.points.size(); ++i) { - const Pose pose = shift_path.points.at(i).point.pose; - const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); - const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); - if (i <= start_segment_idx) { - if (!is_out_of_lane) { - cropped_path.points.push_back(shift_path.points.at(i)); - } - } else { - cropped_path.points.push_back(shift_path.points.at(i)); - } - } - shift_path.points = cropped_path.points; - - // check lane departure - if ( - parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { - continue; - } - - // check collision - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { - continue; - } + const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( + lanelet_map_ptr, shift_path, start_segment_idx); + if (cropped_path.points.empty()) continue; + shift_path.points = cropped_path.points; shift_path.header = planner_data_->route_handler->getRouteHeader(); return pull_out_path; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index c9fa15d855268..74d260fdcabb8 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" @@ -37,6 +38,8 @@ #include using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using motion_utils::calcLateralOffset; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; @@ -66,7 +69,8 @@ StartPlannerModule::StartPlannerModule( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - start_planners_.push_back(std::make_shared(node, *parameters)); + start_planners_.push_back( + std::make_shared(node, *parameters, lane_departure_checker_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -104,7 +108,7 @@ void StartPlannerModule::onFreespacePlannerTimer() BehaviorModuleOutput StartPlannerModule::run() { updateData(); - if (!isActivated()) { + if (!isActivated() || needToPrepareBlinkerBeforeStartDrivingForward()) { return planWaitingApproval(); } @@ -161,6 +165,12 @@ void StartPlannerModule::updateData() DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); } + if ( + planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && + status_.driving_forward && !status_.first_engaged_and_driving_forward_time) { + status_.first_engaged_and_driving_forward_time = clock_->now(); + } + if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving"); @@ -338,11 +348,6 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToRunningState() -{ - return isActivated() && !isWaitingApproval(); -} - BehaviorModuleOutput StartPlannerModule::plan() { if (isWaitingApproval()) { @@ -962,6 +967,18 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } +bool StartPlannerModule::needToPrepareBlinkerBeforeStartDrivingForward() const +{ + if (!status_.first_engaged_and_driving_forward_time) { + return false; + } + const auto first_engaged_and_driving_forward_time = + status_.first_engaged_and_driving_forward_time.value(); + const double elapsed = + rclcpp::Duration(clock_->now() - first_engaged_and_driving_forward_time).seconds(); + return elapsed < parameters_->prepare_time_before_start; +} + bool StartPlannerModule::isStuck() { if (!isStopped()) { @@ -991,107 +1008,70 @@ bool StartPlannerModule::hasFinishedCurrentPath() return is_near_target && isStopped(); } -TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const +TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() { - TurnSignalInfo turn_signal{}; // output + const auto path = getFullPath(); + if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose & start_pose = status_.pull_out_path.start_pose; - const Pose & end_pose = status_.pull_out_path.end_pose; - - // turn on hazard light when backward driving - if (!status_.driving_forward) { - turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; - const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); - turn_signal.desired_start_point = back_start_pose; - turn_signal.required_start_point = back_start_pose; - // pull_out start_pose is same to backward driving end_pose - turn_signal.required_end_point = start_pose; - turn_signal.desired_end_point = start_pose; - return turn_signal; - } + const auto shift_start_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.start_pose.position); + const auto shift_end_idx = + motion_utils::findNearestIndex(path.points, status_.pull_out_path.end_pose.position); + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); - // turn on right signal until passing pull_out end point - const auto path = getFullPath(); - // pull out path does not overlap - const double distance_from_end = - motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position); + const auto is_ignore_signal = [this](const lanelet::Id & id) { + if (!ignore_signal_.has_value()) { + return false; + } + return ignore_signal_.value() == id; + }; - if (path.points.empty()) { - return {}; - } + const auto update_ignore_signal = [this](const lanelet::Id & id, const bool is_ignore) { + return is_ignore ? std::make_optional(id) : std::nullopt; + }; - // calculate lateral offset from pull out target lane center line - lanelet::ConstLanelet closest_road_lane; - const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_->max_back_distance; - const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), - /*forward_only_in_route*/ true); - lanelet::utils::query::getClosestLanelet(road_lanes, start_pose, &closest_road_lane); - const double lateral_offset = - lanelet::utils::getLateralDistanceToCenterline(closest_road_lane, start_pose); - - if (distance_from_end < 0.0 && lateral_offset > parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; - } else if ( - distance_from_end < 0.0 && lateral_offset < -parameters_->th_turn_signal_on_lateral_offset) { - turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; - } else { - turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; + lanelet::Lanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(current_lanes, current_pose, &closest_lanelet); + + if (is_ignore_signal(closest_lanelet.id())) { + return getPreviousModuleOutput().turn_signal_info; } - turn_signal.desired_start_point = start_pose; - turn_signal.required_start_point = start_pose; - turn_signal.desired_end_point = end_pose; - - // check if intersection exists within search length - const bool is_near_intersection = std::invoke([&]() { - const double check_length = parameters_->intersection_search_length; - double accumulated_length = 0.0; - const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position); - for (size_t i = current_idx; i < path.points.size() - 1; ++i) { - const auto & p = path.points.at(i); - for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - return true; - } - } - accumulated_length += tier4_autoware_utils::calcDistance2d(p, path.points.at(i + 1)); - if (accumulated_length > check_length) { - return false; - } + const double current_shift_length = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + constexpr bool egos_lane_is_shifted = true; + constexpr bool is_pull_out = true; + + // In Geometric pull out, the ego stops once and then steers the wheels to the opposite direction. + // This sometimes causes the getBehaviorTurnSignalInfo method to detect the ego as stopped and + // close to complete its shift, so it wrongly turns off the blinkers, this override helps avoid + // this issue. + const bool override_ego_stopped_check = std::invoke([&]() { + if (status_.planner_type != PlannerType::GEOMETRIC) { + return false; } - return false; + constexpr double distance_threshold = 1.0; + const auto stop_point = status_.pull_out_path.partial_paths.front().points.back(); + const double distance_from_ego_to_stop_point = std::abs(motion_utils::calcSignedArcLength( + path.points, stop_point.point.pose.position, current_pose.position)); + return distance_from_ego_to_stop_point < distance_threshold; }); - if (is_near_intersection) { - // offset required end pose with ration to activate turn signal for intersection - turn_signal.required_end_point = std::invoke([&]() { - const double length_start_to_end = - motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position); - const auto ratio = std::clamp( - parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0); - - const double required_end_length = length_start_to_end * ratio; - double accumulated_length = 0.0; - const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position); - for (size_t i = start_idx; i < path.points.size() - 1; ++i) { - accumulated_length += - tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1)); - if (accumulated_length > required_end_length) { - return path.points.at(i).point.pose; - } - } - // not found required end point - return end_pose; - }); - } else { - turn_signal.required_end_point = end_pose; - } + const auto [new_signal, is_ignore] = planner_data_->getBehaviorTurnSignalInfo( + path, shift_start_idx, shift_end_idx, current_lanes, current_shift_length, + status_.driving_forward, egos_lane_is_shifted, override_ego_stopped_check, is_pull_out); + ignore_signal_ = update_ignore_signal(closest_lanelet.id(), is_ignore); - return turn_signal; + const auto original_signal = getPreviousModuleOutput().turn_signal_info; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto output_turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + path, current_pose, current_seg_idx, original_signal, new_signal, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output_turn_signal_info; } bool StartPlannerModule::isSafePath() const @@ -1142,14 +1122,19 @@ bool StartPlannerModule::isSafePath() const const double hysteresis_factor = status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; - utils::parking_departure::updateSafetyCheckTargetObjectsData( - start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); - + std::vector merged_target_object; + merged_target_object.reserve( + target_objects_on_lane.on_current_lane.size() + target_objects_on_lane.on_shoulder_lane.size()); + merged_target_object.insert( + merged_target_object.end(), target_objects_on_lane.on_current_lane.begin(), + target_objects_on_lane.on_current_lane.end()); + merged_target_object.insert( + merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(), + target_objects_on_lane.on_shoulder_lane.end()); return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( - pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, - start_planner_data_.collision_check, planner_data_->parameters, - safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, - hysteresis_factor); + pull_out_path, ego_predicted_path, merged_target_object, start_planner_data_.collision_check, + planner_data_->parameters, safety_check_params_->rss_params, + objects_filtering_params_->use_all_predicted_path, hysteresis_factor); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1253,6 +1238,7 @@ bool StartPlannerModule::planFreespacePath() void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (status_.planner_type == PlannerType::FREESPACE) { + std::cerr << "Freespace planner updated drivable area." << std::endl; const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1271,6 +1257,30 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } +lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const +{ + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + if (pull_out_lanes.empty()) { + return lanelet::ConstLanelets{}; + } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [this](const auto & pull_out_lane) { + return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); + }); + const auto departure_check_lanes = utils::transformToLanelets( + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); + return departure_check_lanes; +} + void StartPlannerModule::setDebugData() const { using marker_utils::createObjectsMarkerArray; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 62a5e06674d4e..8a1526f471775 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -550,6 +550,13 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( if (adj_lane) { return *adj_lane; } + const auto assoc_shoulder = + (turn_direction_ == TurnDirection::LEFT) + ? planner_data_->route_handler_->getLeftLanelet(lane, true /* get_shoulder_lane */) + : planner_data_->route_handler_->getRightLanelet(lane, true); + if (assoc_shoulder) { + return assoc_shoulder.value(); + } return std::nullopt; }); diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 240963309e9a2..c99b3d6d2fa10 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -60,6 +60,11 @@ timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] timeout_ego_stop_for_yield: 3.0 # [s] If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. + # params for suppressing the ego to restart when the ego is close to the next stop position. + restart_suppression: + min_distance_to_stop: 0.5 + max_distance_to_stop: 1.0 + # param for target object filtering object_filtering: crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index b8190cb6124e7..c38a542e18313 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -54,6 +54,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.stop_position_threshold = getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + // param for restart suppression + cp.min_dist_to_stop_for_restart_suppression = + getOrDeclareParameter(node, ns + ".restart_suppression.min_distance_to_stop"); + cp.max_dist_to_stop_for_restart_suppression = + getOrDeclareParameter(node, ns + ".restart_suppression.max_distance_to_stop"); + // param for ego velocity cp.min_slow_down_velocity = getOrDeclareParameter(node, ns + ".slow_down.min_slow_down_velocity"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b8a34bf5a9d00..c2893ce7d1f92 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -197,6 +197,8 @@ CrosswalkModule::CrosswalkModule( collision_info_pub_ = node.create_publisher("~/debug/collision_info", 1); + + vehicle_stop_checker_ = std::make_unique(&node); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -393,7 +395,7 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return {}; } - // Check if the ego should stop beyond the stop line. + // Check if the ego should stop at the stop line or the other points. const bool stop_at_stop_line = dist_ego_to_stop < nearest_stop_info->second && nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; @@ -404,9 +406,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( return createStopFactor(*default_stop_pose, stop_factor_points); } } else { - // Stop beyond the stop line const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, planner_param_.stop_distance_from_object); + ego_path.points, nearest_stop_info->first, + -base_link2front - planner_param_.stop_distance_from_object); if (stop_pose) { return createStopFactor(*stop_pose, stop_factor_points); } @@ -1188,7 +1190,8 @@ void CrosswalkModule::planStop( PathWithLaneId & ego_path, const std::optional & nearest_stop_factor, const std::optional & default_stop_pose, StopReason * stop_reason) { - const auto stop_factor = [&]() -> std::optional { + // Calculate stop factor + auto stop_factor = [&]() -> std::optional { if (nearest_stop_factor) return *nearest_stop_factor; if (default_stop_pose) return createStopFactor(*default_stop_pose); return std::nullopt; @@ -1199,6 +1202,13 @@ void CrosswalkModule::planStop( return; } + // Check if the restart should be suppressed. + const bool suppress_restart = checkRestartSuppression(ego_path, stop_factor); + if (suppress_restart) { + const auto & ego_pose = planner_data_->current_odometry->pose; + stop_factor->stop_pose = ego_pose; + } + // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); planning_utils::appendStopReason(*stop_factor, stop_reason); @@ -1206,4 +1216,22 @@ void CrosswalkModule::planStop( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); } + +bool CrosswalkModule::checkRestartSuppression( + const PathWithLaneId & ego_path, const std::optional & stop_factor) const +{ + const auto is_vehicle_stopped = vehicle_stop_checker_->isVehicleStopped(); + if (!is_vehicle_stopped) { + return false; + } + + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double dist_to_stop = + calcSignedArcLength(ego_path.points, ego_pos, stop_factor->stop_pose.position); + + // NOTE: min_dist_to_stop_for_restart_suppression is supposed to be the same as + // the pid_longitudinal_controller's drive_state_stop_dist. + return planner_param_.min_dist_to_stop_for_restart_suppression < dist_to_stop && + dist_to_stop < planner_param_.max_dist_to_stop_for_restart_suppression; +} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 7f020fbe5422c..77b422f1bd5da 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -115,6 +116,9 @@ class CrosswalkModule : public SceneModuleInterface double stop_distance_from_crosswalk; double far_object_threshold; double stop_position_threshold; + // param for restart suppression + double min_dist_to_stop_for_restart_suppression; + double max_dist_to_stop_for_restart_suppression; // param for ego velocity float min_slow_down_velocity; double max_slow_down_jerk; @@ -404,6 +408,9 @@ class CrosswalkModule : public SceneModuleInterface static geometry_msgs::msg::Polygon createVehiclePolygon( const vehicle_info_util::VehicleInfo & vehicle_info); + bool checkRestartSuppression( + const PathWithLaneId & ego_path, const std::optional & stop_factor) const; + void recordTime(const int step_num) { RCLCPP_INFO_EXPRESSION( @@ -429,6 +436,8 @@ class CrosswalkModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; + std::unique_ptr vehicle_stop_checker_{nullptr}; + // Stop watch StopWatch stop_watch_; diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 31cc035703a7b..d136f4a8598f3 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace behavior_velocity_planner::out_of_lane @@ -58,7 +59,10 @@ std::vector calculate_path_footprints( const auto base_footprint = make_base_footprint(params); std::vector path_footprints; path_footprints.reserve(ego_data.path.points.size()); - for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) { + double length = 0.0; + const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + + params.front_offset + params.extra_front_offset; + for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size() && length < range; ++i) { const auto & path_pose = ego_data.path.points[i].point.pose; const auto angle = tf2::getYaw(path_pose.orientation); const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); @@ -66,6 +70,8 @@ std::vector calculate_path_footprints( for (const auto & p : rotated_footprint.outer()) footprint.emplace_back(p.x() + path_pose.position.x, p.y() + path_pose.position.y); path_footprints.push_back(footprint); + if (i + 1 < ego_data.path.points.size()) + length += tier4_autoware_utils::calcDistance2d(path_pose, ego_data.path.points[i + 1].point); } return path_footprints; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index c5dedf23785ab..25bc2a9718de3 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -24,15 +24,65 @@ namespace behavior_velocity_planner::out_of_lane { + +lanelet::ConstLanelets consecutive_lanelets( + const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) +{ + lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); + const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); + consecutives.insert(consecutives.end(), previous.begin(), previous.end()); + return consecutives; +} + +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler) +{ + lanelet::ConstLanelets missing_lane_change_lanelets; + const auto & routing_graph = *route_handler.getRoutingGraphPtr(); + lanelet::ConstLanelets adjacents; + lanelet::ConstLanelets consecutives; + for (const auto & ll : path_lanelets) { + const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); + std::copy_if( + consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), + [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); + const auto adjacents_of_ll = routing_graph.besides(ll); + std::copy_if( + adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), + [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); + } + std::copy_if( + adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), + [&](const auto & l) { + return !contains_lanelet(missing_lane_change_lanelets, l.id()) && + !contains_lanelet(path_lanelets, l.id()) && contains_lanelet(consecutives, l.id()); + }); + return missing_lane_change_lanelets; +} + +lanelet::ConstLanelets calculate_path_lanelets( + const EgoData & ego_data, const route_handler::RouteHandler & route_handler) +{ + const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); + lanelet::ConstLanelets path_lanelets; + lanelet::BasicLineString2d path_ls; + for (const auto & p : ego_data.path.points) + path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); + for (const auto & dist_lanelet : + lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, path_ls)) { + if (!contains_lanelet(path_lanelets, dist_lanelet.second.id())) + path_lanelets.push_back(dist_lanelet.second); + } + const auto missing_lanelets = get_missing_lane_change_lanelets(path_lanelets, route_handler); + path_lanelets.insert(path_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); + return path_lanelets; +} + lanelet::ConstLanelets calculate_ignored_lanelets( const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler, const PlannerParam & params) { lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets that follows path lanelets - for (const auto & path_lanelet : path_lanelets) - for (const auto & following : route_handler.getRoutingGraphPtr()->following(path_lanelet)) - if (!contains_lanelet(path_lanelets, following.id())) ignored_lanelets.push_back(following); // ignore lanelets directly behind ego const auto behind = planning_utils::calculateOffsetPoint2d(ego_data.pose, params.rear_offset, 0.0); @@ -43,6 +93,16 @@ lanelet::ConstLanelets calculate_ignored_lanelets( const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id()); if (!is_path_lanelet) ignored_lanelets.push_back(l.second); } + // ignore lanelets beyond the last path pose + const auto beyond = planning_utils::calculateOffsetPoint2d( + ego_data.path.points.back().point.pose, params.front_offset, 0.0); + const lanelet::BasicPoint2d beyond_point(beyond.x(), beyond.y()); + const auto beyond_lanelets = lanelet::geometry::findWithin2d( + route_handler.getLaneletMapPtr()->laneletLayer, beyond_point, 0.0); + for (const auto & l : beyond_lanelets) { + const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id()); + if (!is_path_lanelet) ignored_lanelets.push_back(l.second); + } return ignored_lanelets; } @@ -55,26 +115,14 @@ lanelet::ConstLanelets calculate_other_lanelets( const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); const auto lanelets_within_range = lanelet::geometry::findWithin2d( route_handler.getLaneletMapPtr()->laneletLayer, ego_point, - std::max(params.slow_dist_threshold, params.stop_dist_threshold)); - const auto is_overlapped_by_path_lanelets = [&](const auto & l) { - for (const auto & path_ll : path_lanelets) { - if ( - boost::geometry::overlaps( - path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || - boost::geometry::within(path_ll.polygon2d().basicPolygon(), l.polygon2d().basicPolygon()) || - boost::geometry::within(l.polygon2d().basicPolygon(), path_ll.polygon2d().basicPolygon())) { - return true; - } - } - return false; - }; + std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + + params.extra_front_offset); for (const auto & ll : lanelets_within_range) { if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") continue; const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) - other_lanelets.push_back(ll.second); + if (!is_path_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); } return other_lanelets; } diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp index cd0b23768872d..87757a0cb2230 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -33,6 +33,21 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane return l.id() == id; }) != lanelets.end(); }; + +/// @brief calculate lanelets crossed by the ego path +/// @details calculated from the ids of the path msg, the lanelets containing path points +/// @param [in] ego_data data about the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets crossed by the ego vehicle +lanelet::ConstLanelets calculate_path_lanelets( + const EgoData & ego_data, const route_handler::RouteHandler & route_handler); +/// @brief calculate lanelets that may not be crossed by the path but may be overlapped during a +/// lane change +/// @param [in] path_lanelets lanelets driven by the ego vehicle +/// @param [in] route_handler route handler +/// @return lanelets that may be overlapped by a lane change (and are not already in path_lanelets) +lanelet::ConstLanelets get_missing_lane_change_lanelets( + lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler); /// @brief calculate lanelets that should be ignored /// @param [in] ego_data data about the ego vehicle /// @param [in] path_lanelets lanelets driven by the ego vehicle 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 6f74f42af2056..6dfb41ccfbfcb 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 @@ -34,6 +34,7 @@ Overlap calculate_overlap( Overlap overlap; const auto & left_bound = lanelet.leftBound2d().basicLineString(); const auto & right_bound = lanelet.rightBound2d().basicLineString(); + // TODO(Maxime): these intersects (for each path footprint, for each lanelet) are too expensive const auto overlap_left = boost::geometry::intersects(path_footprint, left_bound); const auto overlap_right = boost::geometry::intersects(path_footprint, right_bound); 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 1f6a332e27e8d..ef4434e235244 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 @@ -66,7 +66,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto ego_data.pose = planner_data_->current_odometry->pose; ego_data.path.points = path->points; ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(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; @@ -75,13 +75,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto path_footprints = calculate_path_footprints(ego_data, params_); const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); // Calculate lanelets to ignore and consider - const auto path_lanelets = planning_utils::getLaneletsOnPath( - ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(), - planner_data_->current_odometry->pose); + stopwatch.tic("calculate_lanelets"); + const auto path_lanelets = calculate_path_lanelets(ego_data, *planner_data_->route_handler_); const auto ignored_lanelets = calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); const auto other_lanelets = calculate_other_lanelets( ego_data, path_lanelets, ignored_lanelets, *planner_data_->route_handler_, params_); + const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); debug_data_.footprints = path_footprints; debug_data_.path_lanelets = path_lanelets; @@ -98,6 +98,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto }); if (overlapped_lanelet_it != other_lanelets.end()) { debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); + // TODO(Maxime): we may want to just add the overlapped lane to the ignored lanelets RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module ()\n"); return true; } @@ -183,13 +184,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto RCLCPP_DEBUG( logger_, "Total time = %2.2fus\n" + "\tcalculate_lanelets = %2.0fus\n" "\tcalculate_path_footprints = %2.0fus\n" "\tcalculate_overlapping_ranges = %2.0fus\n" "\tcalculate_decisions = %2.0fus\n" "\tcalc_slowdown_points = %2.0fus\n" "\tinsert_slowdown_points = %2.0fus\n", - total_time_us, calculate_path_footprints_us, calculate_overlapping_ranges_us, - calculate_decisions_us, calc_slowdown_points_us, insert_slowdown_points_us); + total_time_us, calculate_lanelets_us, calculate_path_footprints_us, + calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us, + insert_slowdown_points_us); return true; } diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 22220ccd182a1..2593a289d390d 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -43,6 +43,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 1357b555f0ba4..4f09a8ce2d39c 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -41,6 +41,7 @@ diagnostic_msgs eigen geometry_msgs + jpn_signal_v2i_msgs lanelet2_extension libboost-dev motion_utils diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 5da8df9e70c35..b5d019820bebe 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -108,6 +108,11 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); + sub_traffic_signals_raw_v2i_ = + this->create_subscription( + "~/input/traffic_signals_raw_v2i", 1, + std::bind(&BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I, this, _1), + createSubscriptionOptions(this)); sub_external_velocity_limit_ = this->create_subscription( "~/input/external_velocity_limit_mps", rclcpp::QoS{1}.transient_local(), std::bind(&BehaviorVelocityPlannerNode::onExternalVelocityLimit, this, _1)); @@ -145,6 +150,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio declare_parameter("ego_nearest_dist_threshold"); planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + // is simulation or not + planner_data_.is_simulation = declare_parameter("is_simulation"); + // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter. @@ -330,6 +338,26 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( } } +void BehaviorVelocityPlannerNode::onTrafficSignalsRawV2I( + const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_); + + for (const auto & car_light : msg->car_lights) { + // If the time to red is not available, skip it + if (!car_light.has_max_rest_time || !car_light.has_min_rest_time) { + continue; + } + + for (const auto & state : car_light.states) { + TrafficSignalTimeToRedStamped time_to_red; + time_to_red.stamp = msg->header.stamp; + time_to_red.time_to_red = car_light.min_rest_time_to_red; + planner_data_.traffic_light_time_to_red_id_map[state.traffic_signal_id] = time_to_red; + } + } +} + void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { std::lock_guard lock(mutex_); diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index ded1d08700a1d..29a72a5bc2c8f 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -27,6 +27,8 @@ #include #include #include +#include +#include #include #include #include @@ -71,6 +73,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_traffic_signals_; + rclcpp::Subscription::SharedPtr + sub_traffic_signals_raw_v2i_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; @@ -86,6 +90,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); void onTrafficSignals( const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + void onTrafficSignalsRawV2I(const jpn_signal_v2i_msgs::msg::TrafficLightInfo::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); diff --git a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp index 491df4c7a8766..935530b52e175 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ament_index_cpp/get_package_share_directory.hpp" #include "node.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager.hpp" -#include "planning_interface_test_manager/planning_interface_test_manager_utils.hpp" + +#include +#include +#include #include @@ -78,6 +79,7 @@ std::shared_ptr generateNode() std::vector params; params.emplace_back("launch_modules", module_names); + params.emplace_back("is_simulation", false); node_options.parameter_overrides(params); test_utils::updateNodeOptions( diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2ea9f6ed2648b..d1f0c39c71205 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -40,6 +40,7 @@ #include #include +#include #include #include #include @@ -78,8 +79,13 @@ struct PlannerData // other internal data std::map traffic_light_id_map; std::optional external_velocity_limit; + std::map traffic_light_time_to_red_id_map; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; + // This variable is used when the Autoware's behavior has to depend on whether it's simulation or + // not. + bool is_simulation = false; + // velocity smoother std::shared_ptr velocity_smoother_; // route handler @@ -132,6 +138,15 @@ struct PlannerData } return std::make_shared(traffic_light_id_map.at(id)); } + + std::optional getRestTimeToRedSignal(const int id) const + { + try { + return traffic_light_time_to_red_id_map.at(id); + } catch (std::out_of_range &) { + return std::nullopt; + } + } }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index e042338c53485..87c68506c73ce 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -56,6 +56,12 @@ struct TrafficSignalStamped autoware_perception_msgs::msg::TrafficSignal signal; }; +struct TrafficSignalTimeToRedStamped +{ + builtin_interfaces::msg::Time stamp; + double time_to_red; +}; + using Pose = geometry_msgs::msg::Pose; using Point2d = tier4_autoware_utils::Point2d; using LineString2d = tier4_autoware_utils::LineString2d; diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 784b7cabfe9ad..4d2563bc2c2ff 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -28,6 +28,7 @@ eigen geometry_msgs interpolation + jpn_signal_v2i_msgs lanelet2_extension motion_utils motion_velocity_smoother diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 21d1b5f1e3e4f..0af59417a23b2 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -26,6 +26,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -312,6 +313,40 @@ void calculateMinAndMaxVelFromCovariance( dynamic_obstacle.max_velocity_mps = max_velocity; } +double convertDurationToDouble(const builtin_interfaces::msg::Duration & duration) +{ + return duration.sec + duration.nanosec / 1e9; +} + +// Create a path leading up to a specified prediction time +std::vector createPathToPredictionTime( + const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) +{ + // Calculate the number of poses to include based on the prediction time and the time step between + // poses + const double time_step_seconds = convertDurationToDouble(predicted_path.time_step); + if (time_step_seconds < std::numeric_limits::epsilon()) { + // Handle the case where time_step_seconds is zero or too close to zero + RCLCPP_WARN_STREAM( + rclcpp::get_logger("run_out: createPathToPredictionTime"), + "time_step of the path is too close to zero. Use the input path"); + const std::vector input_path( + predicted_path.path.begin(), predicted_path.path.end()); + return input_path; + } + const size_t poses_to_include = + std::min(static_cast(prediction_time / time_step_seconds), predicted_path.path.size()); + + // Construct the path to the specified prediction time + std::vector path_to_prediction_time; + path_to_prediction_time.reserve(poses_to_include); + for (size_t i = 0; i < poses_to_include; ++i) { + path_to_prediction_time.push_back(predicted_path.path[i]); + } + + return path_to_prediction_time; +} + } // namespace DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject( @@ -343,8 +378,7 @@ std::vector DynamicObstacleCreatorForObject::createDynamicObsta for (const auto & path : predicted_object.kinematics.predicted_paths) { PredictedPath predicted_path; predicted_path.confidence = path.confidence; - predicted_path.path.resize(path.path.size()); - std::copy(path.path.cbegin(), path.path.cend(), predicted_path.path.begin()); + predicted_path.path = createPathToPredictionTime(path, param_.max_prediction_time); dynamic_obstacle.predicted_paths.emplace_back(predicted_path); } diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index 23746a61b6043..191af7752202d 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -7,3 +7,9 @@ yellow_lamp_period: 2.75 enable_pass_judge: true enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + + v2i: + use_rest_time: true + last_time_allowed_to_pass: 2.0 # relative time against at the time of turn to red + velocity_threshold: 0.5 # change the decision logic whether the current velocity is faster or not + required_time_to_departure: 3.0 # prevent low speed pass diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 908627540bdcc..521ac677b24a1 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -43,6 +43,14 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); + planner_param_.v2i_use_rest_time = getOrDeclareParameter(node, ns + ".v2i.use_rest_time"); + planner_param_.v2i_last_time_allowed_to_pass = + getOrDeclareParameter(node, ns + ".v2i.last_time_allowed_to_pass"); + planner_param_.v2i_velocity_threshold = + getOrDeclareParameter(node, ns + ".v2i.velocity_threshold"); + planner_param_.v2i_required_time_to_departure = + getOrDeclareParameter(node, ns + ".v2i.required_time_to_departure"); + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index a74ff8c0cb5e8..83443bc726bbd 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -228,11 +228,35 @@ bool TrafficLightModule::modifyPathVelocity(PathWithLaneId * path, StopReason * first_ref_stop_path_point_index_ = stop_line_point_idx; - // Check if stop is coming. + // Update traffic signal information + updateTrafficSignal(); + + const bool is_unknown_signal = isUnknownSignal(looking_tl_state_); + const bool is_signal_timed_out = isTrafficSignalTimedOut(); const bool is_stop_signal = isStopSignal(); + const bool is_stop_required = is_unknown_signal || is_signal_timed_out || is_stop_signal; + + // Decide if stop or proceed using the remaining time to red signal + // If the upcoming traffic signal color is unknown or timed out, do not use the time to red and + // stop for the traffic signal + const auto rest_time_to_red_signal = + planner_data_->getRestTimeToRedSignal(traffic_light_reg_elem_.id()); + const bool is_time_to_red_signal_available = + (rest_time_to_red_signal.has_value() && !isDataTimeout(rest_time_to_red_signal->stamp)); + + if (planner_param_.v2i_use_rest_time && is_time_to_red_signal_available && !is_stop_required) { + if (!canPassStopLineBeforeRed(*rest_time_to_red_signal, signed_arc_length_to_stop_point)) { + *path = insertStopPose(input_path, stop_line_point_idx, stop_line_point, stop_reason); + is_prev_state_stop_ = true; + return true; + } + + is_prev_state_stop_ = false; + return true; + } // Update stop signal received time - if (is_stop_signal) { + if (is_stop_required) { if (!stop_signal_received_time_ptr_) { stop_signal_received_time_ptr_ = std::make_unique