diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d3d9149cd2c8d..f7603af847d1f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,5 +1,3 @@ -### Copied from .github/CODEOWNERS-manual ### - ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -23,6 +21,7 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp +common/mission_planner_rviz_plugin/** isamu.takagi@tier4.jp common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @@ -32,7 +31,7 @@ common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +common/tensorrt_common/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp @@ -55,7 +54,7 @@ common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp -common/traffic_light_utils/** mingyu.li@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp +common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp control/autonomous_emergency_braking/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -76,6 +75,7 @@ control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp +evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp evaluator/tier4_metrics_rviz_plugin/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp @@ -118,9 +118,9 @@ perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihir perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp -perception/detected_object_validation/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp +perception/detected_object_validation/** dai.nguyen@tier4.jp shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/detection_by_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp shintaro.tomie@tier4.jp taichi.higashide@tier4.jp perception/euclidean_cluster/** yukihiro.saito@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/image_projection_based_fusion/** dai.nguyen@tier4.jp koji.minoda@tier4.jp kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -130,8 +130,8 @@ perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu perception/lidar_centerpoint/** kenzo.lobos@tier4.jp koji.minoda@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp -perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/multi_object_tracker/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp perception/object_velocity_splitter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp perception/occupancy_grid_map_outlier_filter/** abrahammonrroy@yahoo.com yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -139,17 +139,17 @@ perception/probabilistic_occupancy_grid_map/** mamoru.sobue@tier4.jp yoshi.ri@ti perception/radar_crossing_objects_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/radar_fusion_to_detected_object/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/radar_object_clustering/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -perception/radar_object_tracker/** satoshi.tanaka@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/radar_object_tracker/** satoshi.tanaka@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/radar_tracks_msgs_converter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp perception/shape_estimation/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -perception/tensorrt_classifier/** mingyu.li@tier4.jp -perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp -perception/tracking_object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +perception/tensorrt_classifier/** kotaro.uetake@tier4.jp shunsuke.miura@tier4.jp +perception/tensorrt_yolox/** dan.umeda@tier4.jp manato.hirabayashi@tier4.jp +perception/tracking_object_merger/** taekjin.lee@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp -perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp +perception/traffic_light_fine_detector/** shintaro.tomie@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp @@ -186,7 +186,7 @@ planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -217,7 +217,7 @@ sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp -sensing/tier4_pcl_extensions/** ryu.yamamoto@tier4.jp +sensing/tier4_pcl_extensions/** david.wong@tier4.jp kenzo.lobos@tier4.jp ryu.yamamoto@tier4.jp sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp @@ -232,10 +232,12 @@ system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.j system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp -system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp +system/duplicated_node_checker/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp +system/hazard_status_converter/** isamu.takagi@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp +system/mrm_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp @@ -245,3 +247,5 @@ vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp vehicle/steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp + +### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index e9db0d140d947..e4231a12a6add 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -24,7 +24,6 @@ jobs: rosdistro: - humble container-suffix: - - "" - -cuda include: - rosdistro: humble @@ -74,38 +73,3 @@ jobs: - name: Check disk space after build run: df -h - - clang-tidy-differential: - runs-on: [self-hosted, linux, X64] - container: ghcr.io/autowarefoundation/autoware-universe:humble-latest-cuda - needs: build-and-test-differential - steps: - - name: Check out repository - uses: actions/checkout@v3 - with: - fetch-depth: 0 - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 7568c89908fd7..a6eea766cd80d 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -9,7 +9,7 @@ on: jobs: build-and-test: if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false @@ -27,14 +27,6 @@ jobs: - name: Check out repository uses: actions/checkout@v3 - - name: Free disk space (Ubuntu) - uses: jlumbroso/free-disk-space@v1.2.0 - with: - tool-cache: false - dotnet: false - swap-storage: false - large-packages: false - - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 diff --git a/.github/workflows/json-schema-check.yaml b/.github/workflows/json-schema-check.yaml index 332d8eebaa518..77ce4576b4952 100644 --- a/.github/workflows/json-schema-check.yaml +++ b/.github/workflows/json-schema-check.yaml @@ -2,13 +2,26 @@ name: json-schema-check on: pull_request: - paths: - - "**/schema/*.schema.json" - - "**/config/*.param.yaml" workflow_dispatch: jobs: + check-if-relevant-files-changed: + runs-on: ubuntu-latest + outputs: + run-check: ${{ steps.paths_filter.outputs.json_or_yaml }} + steps: + - uses: actions/checkout@v3 + - uses: dorny/paths-filter@v3 + id: paths_filter + with: + filters: | + json_or_yaml: + - '**/schema/*.schema.json' + - '**/config/*.param.yaml' + json-schema-check: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'true' runs-on: ubuntu-latest steps: - name: Check out repository @@ -16,3 +29,11 @@ jobs: - name: Run json-schema-check uses: autowarefoundation/autoware-github-actions/json-schema-check@v1 + + no-relevant-changes: + needs: check-if-relevant-files-changed + if: needs.check-if-relevant-files-changed.outputs.run-check == 'false' + runs-on: ubuntu-latest + steps: + - name: Dummy step + run: echo "No relevant changes, passing check" diff --git a/README.md b/README.md index d429cc035df1d..23d0b172554fd 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,16 @@ -# autoware.universe +# Autoware Universe -For Autoware's general documentation, see [Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/). +## Welcome to Autoware Universe -For detailed documents of Autoware Universe components, see [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/). +Autoware Universe serves as a foundational pillar within the Autoware ecosystem, playing a critical role in enhancing the core functionalities of autonomous driving technologies. +This repository is a pivotal element of the Autoware Core/Universe concept, managing a wide array of packages that significantly extend the capabilities of autonomous vehicles. ---- +![autoware_universe_front](docs/assets/images/autoware_universe_front.png) + +## Getting Started + +To dive into the vast world of Autoware and understand how Autoware Universe fits into the bigger picture, we recommend starting with the [Autoware Documentation](https://autowarefoundation.github.io/autoware-documentation/). This resource provides a thorough overview of the Autoware ecosystem, guiding you through its components, functionalities, and how to get started with development. + +### Explore Autoware Universe documentation + +For those looking to explore the specifics of Autoware Universe components, the [Autoware Universe Documentation](https://autowarefoundation.github.io/autoware.universe/), deployed with MKDocs, offers detailed insights. diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index cc0ada00fa41b..da075b2648937 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -23,7 +23,7 @@ ament_lint_auto autoware_lint_common - ament_cmake + ament_cmake_auto ament_cmake diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index b519a0ddce86a..146f3688513cd 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -4,26 +4,8 @@ project(component_interface_specs) find_package(autoware_cmake REQUIRED) autoware_package() -include_directories( - include - SYSTEM - ${rclcpp_INCLUDE_DIRS} - ${rosidl_runtime_cpp_INCLUDE_DIRS} - ${rcl_INCLUDE_DIRS} - ${autoware_adapi_v1_msgs_INCLUDE_DIRS} - ${autoware_auto_planning_msgs_INCLUDE_DIRS} - ${autoware_planning_msgs_INCLUDE_DIRS} - ${autoware_auto_vehicle_msgs_INCLUDE_DIRS} - ${tier4_control_msgs_INCLUDE_DIRS} - ${nav_msgs_INCLUDE_DIRS} - ${tier4_system_msgs_INCLUDE_DIRS} - ${tier4_vehicle_msgs_INCLUDE_DIRS} - ${autoware_auto_perception_msgs_INCLUDE_DIRS} - ${tier4_map_msgs_INCLUDE_DIRS} -) - if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_component_interface_specs + ament_auto_add_gtest(gtest_${PROJECT_NAME} test/gtest_main.cpp test/test_planning.cpp test/test_control.cpp @@ -33,9 +15,6 @@ if(BUILD_TESTING) test/test_perception.cpp test/test_vehicle.cpp ) - target_include_directories(test_component_interface_specs - PRIVATE include - ) endif() ament_auto_package() diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 567ea9d69f2a7..9efd8c871f68f 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,77 +17,48 @@ #include -#include -#include -#include -#include #include #include +#include +#include +#include +#include namespace planning_interface { -struct SetRoutePoints +struct SetLaneletRoute { - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "/planning/mission_planning/set_route_points"; + using Service = tier4_planning_msgs::srv::SetLaneletRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/set_lanelet_route"; }; -struct SetRoute +struct SetWaypointRoute { - using Service = autoware_adapi_v1_msgs::srv::SetRoute; - static constexpr char name[] = "/planning/mission_planning/set_route"; -}; - -struct ChangeRoutePoints -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "/planning/mission_planning/change_route_points"; -}; - -struct ChangeRoute -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoute; - static constexpr char name[] = "/planning/mission_planning/change_route"; + using Service = tier4_planning_msgs::srv::SetWaypointRoute; + static constexpr char name[] = + "/planning/mission_planning/route_selector/main/set_waypoint_route"; }; struct ClearRoute { - using Service = autoware_adapi_v1_msgs::srv::ClearRoute; - static constexpr char name[] = "/planning/mission_planning/clear_route"; + using Service = tier4_planning_msgs::srv::ClearRoute; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/clear_route"; }; struct RouteState { - using Message = autoware_adapi_v1_msgs::msg::RouteState; - static constexpr char name[] = "/planning/mission_planning/route_state"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; -}; - -struct Route -{ - using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/route"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; -}; - -struct NormalRoute -{ - using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/normal_route"; + using Message = tier4_planning_msgs::msg::RouteState; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/state"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -struct MrmRoute +struct LaneletRoute { using Message = autoware_planning_msgs::msg::LaneletRoute; - static constexpr char name[] = "/planning/mission_planning/mrm_route"; + static constexpr char name[] = "/planning/mission_planning/route_selector/main/route"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 67279d0ae2b7f..1ad5f410a814a 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -9,14 +9,8 @@ Apache License 2.0 ament_cmake_auto - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test autoware_cmake - ament_cmake_core - ament_cmake_test - autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs @@ -28,9 +22,11 @@ rosidl_runtime_cpp tier4_control_msgs tier4_map_msgs + tier4_planning_msgs tier4_system_msgs tier4_vehicle_msgs - ament_cmake_ros + + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/common/component_interface_specs/test/test_planning.cpp b/common/component_interface_specs/test/test_planning.cpp index 8c504cb119854..c9cf353de5f34 100644 --- a/common/component_interface_specs/test/test_planning.cpp +++ b/common/component_interface_specs/test/test_planning.cpp @@ -27,26 +27,8 @@ TEST(planning, interface) } { - using planning_interface::Route; - Route route; - size_t depth = 1; - EXPECT_EQ(route.depth, depth); - EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); - EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - } - - { - using planning_interface::NormalRoute; - NormalRoute route; - size_t depth = 1; - EXPECT_EQ(route.depth, depth); - EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); - EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); - } - - { - using planning_interface::MrmRoute; - MrmRoute route; + using planning_interface::LaneletRoute; + LaneletRoute route; size_t depth = 1; EXPECT_EQ(route.depth, depth); EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); diff --git a/common/mission_planner_rviz_plugin/CMakeLists.txt b/common/mission_planner_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..2b06e6db3e70d --- /dev/null +++ b/common/mission_planner_rviz_plugin/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(mission_planner_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/mrm_goal.cpp + src/route_selector_panel.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package() diff --git a/common/mission_planner_rviz_plugin/README.md b/common/mission_planner_rviz_plugin/README.md new file mode 100644 index 0000000000000..59d36a0a2f840 --- /dev/null +++ b/common/mission_planner_rviz_plugin/README.md @@ -0,0 +1,18 @@ +# mission_planner_rviz_plugin + +## MrmGoalTool + +This is a copy of `rviz_default_plugins::tools::GoalTool`. Used together with the RouteSelectorPanel to set the MRM route. +After adding the tool, change the topic name to `/rviz/route_selector/mrm/goal` from the topic property panel in rviz. + +## RouteSelectorPanel + +This panel shows the main and mrm route state in the route_selector and the route states in the mission_planner. +Additionally, it provides clear and set functions for each main route and mrm route. + +| Trigger | Action | +| -------------------------------------- | ------------------------------------------------------------------------ | +| main route clear button | call `/planning/mission_planning/route_selector/main/clear_route` | +| mrm route clear button | call `/planning/mission_planning/route_selector/mrm/clear_route` | +| `/rviz/route_selector/main/goal` topic | call `/planning/mission_planning/route_selector/main/set_waypoint_route` | +| `/rviz/route_selector/mrm/goal` topic | call `/planning/mission_planning/route_selector/mrm/set_waypoint_route` | diff --git a/common/mission_planner_rviz_plugin/package.xml b/common/mission_planner_rviz_plugin/package.xml new file mode 100644 index 0000000000000..e45cf2739f260 --- /dev/null +++ b/common/mission_planner_rviz_plugin/package.xml @@ -0,0 +1,29 @@ + + + + mission_planner_rviz_plugin + 0.0.0 + The mission_planner_rviz_plugin package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + rclcpp + rviz_common + rviz_default_plugins + tier4_planning_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/mission_planner_rviz_plugin/plugins/plugin_description.xml b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..c8285fcc604d7 --- /dev/null +++ b/common/mission_planner_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,8 @@ + + + MrmGoalTool + + + RouteSelectorPanel + + diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.cpp b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp new file mode 100644 index 0000000000000..ef5529abfb4a7 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/mrm_goal.cpp @@ -0,0 +1,34 @@ +// 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 "mrm_goal.hpp" + +namespace rviz_plugins +{ + +MrmGoalTool::MrmGoalTool() +{ + shortcut_key_ = 'm'; +} + +void MrmGoalTool::onInitialize() +{ + GoalTool::onInitialize(); + setName("MRM Goal Pose"); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmGoalTool, rviz_common::Tool) diff --git a/common/mission_planner_rviz_plugin/src/mrm_goal.hpp b/common/mission_planner_rviz_plugin/src/mrm_goal.hpp new file mode 100644 index 0000000000000..e16b0abf0bab3 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/mrm_goal.hpp @@ -0,0 +1,34 @@ +// 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 MRM_GOAL_HPP_ +#define MRM_GOAL_HPP_ + +#include + +namespace rviz_plugins +{ + +class MrmGoalTool : public rviz_default_plugins::tools::GoalTool +{ + Q_OBJECT + +public: + MrmGoalTool(); + void onInitialize() override; +}; + +} // namespace rviz_plugins + +#endif // MRM_GOAL_HPP_ diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp new file mode 100644 index 0000000000000..b4b376b1e76ec --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/route_selector_panel.cpp @@ -0,0 +1,148 @@ +// 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 "route_selector_panel.hpp" + +#include +#include + +#include +#include + +namespace rviz_plugins +{ + +QString to_string_state(const RouteState & state) +{ + // clang-format off + switch (state.state) { + case RouteState::UNKNOWN: return "unknown"; + case RouteState::INITIALIZING: return "initializing"; + case RouteState::UNSET: return "unset"; + case RouteState::ROUTING: return "routing"; + case RouteState::SET: return "set"; + case RouteState::REROUTING: return "rerouting"; + case RouteState::ARRIVED: return "arrived"; + case RouteState::ABORTED: return "aborted"; + case RouteState::INTERRUPTED: return "interrupted"; + default: return "-----"; + } + // clang-format on +} + +RouteSelectorPanel::RouteSelectorPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + main_state_ = new QLabel("unknown"); + main_clear_ = new QPushButton("clear"); + mrm_state_ = new QLabel("unknown"); + mrm_clear_ = new QPushButton("clear"); + planner_state_ = new QLabel("unknown"); + + connect(main_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMainClear); + connect(mrm_clear_, &QPushButton::clicked, this, &RouteSelectorPanel::onMrmClear); + + const auto layout = new QGridLayout(); + setLayout(layout); + layout->addWidget(new QLabel("main"), 0, 0); + layout->addWidget(main_state_, 0, 1); + layout->addWidget(main_clear_, 0, 2); + layout->addWidget(new QLabel("mrm"), 1, 0); + layout->addWidget(mrm_state_, 1, 1); + layout->addWidget(mrm_clear_, 1, 2); + layout->addWidget(new QLabel("planner"), 2, 0); + layout->addWidget(planner_state_, 2, 1); +} + +void RouteSelectorPanel::onInitialize() +{ + auto lock = getDisplayContext()->getRosNodeAbstraction().lock(); + auto node = lock->get_raw_node(); + + const auto durable_qos = rclcpp::QoS(1).transient_local(); + + sub_main_goal_ = node->create_subscription( + "/rviz/route_selector/main/goal", rclcpp::QoS(1), + std::bind(&RouteSelectorPanel::onMainGoal, this, std::placeholders::_1)); + sub_mrm_goal_ = node->create_subscription( + "/rviz/route_selector/mrm/goal", rclcpp::QoS(1), + std::bind(&RouteSelectorPanel::onMrmGoal, this, std::placeholders::_1)); + sub_main_state_ = node->create_subscription( + "/planning/mission_planning/route_selector/main/state", durable_qos, + std::bind(&RouteSelectorPanel::onMainState, this, std::placeholders::_1)); + sub_mrm_state_ = node->create_subscription( + "/planning/mission_planning/route_selector/mrm/state", durable_qos, + std::bind(&RouteSelectorPanel::onMrmState, this, std::placeholders::_1)); + sub_planner_state_ = node->create_subscription( + "/planning/mission_planning/state", durable_qos, + std::bind(&RouteSelectorPanel::onPlannerState, this, std::placeholders::_1)); + + cli_main_clear_ = + node->create_client("/planning/mission_planning/route_selector/main/clear_route"); + cli_mrm_clear_ = + node->create_client("/planning/mission_planning/route_selector/mrm/clear_route"); + cli_main_set_waypoint_ = node->create_client( + "/planning/mission_planning/route_selector/main/set_waypoint_route"); + cli_mrm_set_waypoint_ = node->create_client( + "/planning/mission_planning/route_selector/mrm/set_waypoint_route"); +} + +void RouteSelectorPanel::onMainGoal(const PoseStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->header = msg->header; + req->goal_pose = msg->pose; + req->allow_modification = true; + cli_main_set_waypoint_->async_send_request(req); +} + +void RouteSelectorPanel::onMrmGoal(const PoseStamped::ConstSharedPtr msg) +{ + const auto req = std::make_shared(); + req->header = msg->header; + req->goal_pose = msg->pose; + req->allow_modification = true; + cli_mrm_set_waypoint_->async_send_request(req); +} + +void RouteSelectorPanel::onMainState(RouteState::ConstSharedPtr msg) +{ + main_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onMrmState(RouteState::ConstSharedPtr msg) +{ + mrm_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onPlannerState(RouteState::ConstSharedPtr msg) +{ + planner_state_->setText(to_string_state(*msg)); +} + +void RouteSelectorPanel::onMainClear() +{ + const auto req = std::make_shared(); + cli_main_clear_->async_send_request(req); +} + +void RouteSelectorPanel::onMrmClear() +{ + const auto req = std::make_shared(); + cli_mrm_clear_->async_send_request(req); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::RouteSelectorPanel, rviz_common::Panel) diff --git a/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp b/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp new file mode 100644 index 0000000000000..99101730661e0 --- /dev/null +++ b/common/mission_planner_rviz_plugin/src/route_selector_panel.hpp @@ -0,0 +1,75 @@ +// 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 ROUTE_SELECTOR_PANEL_HPP_ +#define ROUTE_SELECTOR_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ + +using geometry_msgs::msg::PoseStamped; +using tier4_planning_msgs::msg::RouteState; +using tier4_planning_msgs::srv::ClearRoute; +using tier4_planning_msgs::srv::SetWaypointRoute; + +class RouteSelectorPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit RouteSelectorPanel(QWidget * parent = nullptr); + void onInitialize() override; + +private: + QLabel * main_state_; + QLabel * mrm_state_; + QLabel * planner_state_; + QPushButton * main_clear_; + QPushButton * mrm_clear_; + + rclcpp::Subscription::SharedPtr sub_main_goal_; + rclcpp::Subscription::SharedPtr sub_mrm_goal_; + void onMainGoal(const PoseStamped::ConstSharedPtr msg); + void onMrmGoal(const PoseStamped::ConstSharedPtr msg); + + rclcpp::Subscription::SharedPtr sub_main_state_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + rclcpp::Subscription::SharedPtr sub_planner_state_; + void onMainState(RouteState::ConstSharedPtr msg); + void onMrmState(RouteState::ConstSharedPtr msg); + void onPlannerState(RouteState::ConstSharedPtr msg); + + rclcpp::Client::SharedPtr cli_main_clear_; + rclcpp::Client::SharedPtr cli_mrm_clear_; + rclcpp::Client::SharedPtr cli_main_set_waypoint_; + rclcpp::Client::SharedPtr cli_mrm_set_waypoint_; + +private Q_SLOTS: + void onMainClear(); + void onMrmClear(); +}; + +} // namespace rviz_plugins + +#endif // ROUTE_SELECTOR_PANEL_HPP_ diff --git a/common/tensorrt_common/package.xml b/common/tensorrt_common/package.xml index 7d3995f93f7fe..f5a3896b55881 100644 --- a/common/tensorrt_common/package.xml +++ b/common/tensorrt_common/package.xml @@ -6,7 +6,6 @@ Taichi Higashide Daisuke Nishimatsu - Daisuke Nishimatsu Dan Umeda Manato Hirabayashi 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/traffic_light_utils/package.xml b/common/traffic_light_utils/package.xml index 37b4d46ce356a..7adb856c3c447 100644 --- a/common/traffic_light_utils/package.xml +++ b/common/traffic_light_utils/package.xml @@ -4,7 +4,7 @@ traffic_light_utils 0.1.0 The traffic_light_utils package - Mingyu Li + Kotaro Uetake Shunsuke Miura Satoshi Ota Apache License 2.0 diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp index fa8261e9c3f95..fab8b9f6f888b 100644 --- a/control/control_validator/src/utils.cpp +++ b/control/control_validator/src/utils.cpp @@ -104,10 +104,10 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // If first point of predicted_trajectory is in front of start of trajectory, erase points which // are in front of trajectory start point and insert pNew along the predicted_trajectory - // predicted_trajectory: 　　　　p1-----p2-----p3----//------pN + // predicted_trajectory: p1-----p2-----p3----//------pN // trajectory: t1--------//------tN // ↓ - // predicted_trajectory: 　　　　 pNew--p3----//------pN + // predicted_trajectory: pNew--p3----//------pN // trajectory: t1--------//------tN auto predicted_trajectory_point_removed = removeFrontTrajectoryPoint( trajectory_points, modified_trajectory_points, predicted_trajectory_points); @@ -119,7 +119,7 @@ Trajectory alignTrajectoryWithReferenceTrajectory( // If last point of predicted_trajectory is behind of end of trajectory, erase points which are // behind trajectory last point and insert pNew along the predicted_trajectory - // predicted_trajectory: 　　　　p1-----//------pN-2-----pN-1-----pN + // predicted_trajectory: p1-----//------pN-2-----pN-1-----pN // trajectory: t1-----//-----tN-1--tN // ↓ // predicted_trajectory: p1-----//------pN-2-pNew diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index b8ee8ad79a33d..4674ebdadb51d 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -4,6 +4,21 @@ `joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. +## Usage + +### ROS 2 launch + +```bash +# With default config (ds4) +ros2 launch joy_controller joy_controller.launch.xml + +# Default config but select from the existing parameter files +ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox + +# Override the param file +ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml +``` + ## Input / Output ### Input topics diff --git a/control/joy_controller/config/joy_controller.param.yaml b/control/joy_controller/config/joy_controller_ds4.param.yaml similarity index 92% rename from control/joy_controller/config/joy_controller.param.yaml rename to control/joy_controller/config/joy_controller_ds4.param.yaml index c9fe348867e68..73a5d028985c5 100644 --- a/control/joy_controller/config/joy_controller.param.yaml +++ b/control/joy_controller/config/joy_controller_ds4.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - joy_type: $(var joy_type) + joy_type: DS4 update_rate: 10.0 accel_ratio: 3.0 brake_ratio: 5.0 diff --git a/control/joy_controller/config/joy_controller_g29.param.yaml b/control/joy_controller/config/joy_controller_g29.param.yaml new file mode 100644 index 0000000000000..c28a6c01d1326 --- /dev/null +++ b/control/joy_controller/config/joy_controller_g29.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + joy_type: G29 + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + raw_control: false + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/joy_controller/config/joy_controller_p65.param.yaml b/control/joy_controller/config/joy_controller_p65.param.yaml new file mode 100644 index 0000000000000..4b13348f95fb6 --- /dev/null +++ b/control/joy_controller/config/joy_controller_p65.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + joy_type: P65 + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + raw_control: false + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/joy_controller/config/joy_controller_xbox.param.yaml b/control/joy_controller/config/joy_controller_xbox.param.yaml new file mode 100644 index 0000000000000..7b45c2c164454 --- /dev/null +++ b/control/joy_controller/config/joy_controller_xbox.param.yaml @@ -0,0 +1,16 @@ +/**: + ros__parameters: + joy_type: XBOX + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + raw_control: false + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/joy_controller/launch/joy_controller.launch.xml index c392ca100a9c0..5236077680d0d 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/joy_controller/launch/joy_controller.launch.xml @@ -1,5 +1,4 @@ - @@ -13,7 +12,7 @@ - + diff --git a/control/joy_controller/launch/joy_controller_param_selection.launch.xml b/control/joy_controller/launch/joy_controller_param_selection.launch.xml new file mode 100644 index 0000000000000..d8e3d0bfe8b25 --- /dev/null +++ b/control/joy_controller/launch/joy_controller_param_selection.launch.xml @@ -0,0 +1,7 @@ + + + + + + + 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..be2411cd3268b 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..811e1652fcb4a 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,92 @@ 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); + // Fuse lanelets into a single BasicPolygon2d + auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional { + if (lanelets_distance_pair.empty()) return std::nullopt; + if (lanelets_distance_pair.size() == 1) + return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + + lanelet::BasicPolygon2d merged_polygon = + lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { + const auto & route_lanelet = lanelets_distance_pair.at(i).second; + const auto & poly = route_lanelet.polygon2d().basicPolygon(); + + std::vector lanelet_union_temp; + boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); + + // Update merged_polygon by accumulating all merged results + merged_polygon.clear(); + for (const auto & temp_poly : lanelet_union_temp) { + merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end()); + } + } + if (merged_polygon.empty()) return std::nullopt; + return merged_polygon; + }(); + + 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 +452,5 @@ bool LaneDepartureChecker::willCrossBoundary( } return false; } + } // namespace lane_departure_checker diff --git a/docs/assets/images/autoware_universe_front.png b/docs/assets/images/autoware_universe_front.png new file mode 100644 index 0000000000000..e03e35d12f78b Binary files /dev/null and b/docs/assets/images/autoware_universe_front.png differ diff --git a/evaluator/perception_online_evaluator/CMakeLists.txt b/evaluator/perception_online_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..f9cc0f4fa256c --- /dev/null +++ b/evaluator/perception_online_evaluator/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.14) +project(perception_online_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +find_package(glog REQUIRED) + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/metrics_calculator.cpp + src/${PROJECT_NAME}_node.cpp + src/metrics/deviation_metrics.cpp + src/utils/marker_utils.cpp + src/utils/objects_filtering.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "perception_diagnostics::PerceptionOnlineEvaluatorNode" + EXECUTABLE ${PROJECT_NAME} +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "perception_diagnostics::MotionEvaluatorNode" + EXECUTABLE motion_evaluator +) + +target_link_libraries(${PROJECT_NAME}_node glog::glog) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + test/test_perception_online_evaluator_node.cpp + ) + target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME}_node + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md new file mode 100644 index 0000000000000..b801e5f418cef --- /dev/null +++ b/evaluator/perception_online_evaluator/README.md @@ -0,0 +1,43 @@ +# Perception Evaluator + +A node for evaluating the output of perception systems. + +## Purpose + +This module allows for the evaluation of how accurately perception results are generated without the need for annotations. It is capable of confirming performance and can evaluate results from a few seconds prior, enabling online execution. + +## Inner-workings / Algorithms + +- Calculates lateral deviation between the predicted path and the actual traveled trajectory. +- Calculates lateral deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of lateral position recognition. +- Calculates yaw deviation between the smoothed traveled trajectory and the perceived position to evaluate the stability of yaw recognition. + +## Inputs / Outputs + +| Name | Type | Description | +| ----------------- | ------------------------------------------------------ | ------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | +| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | + +## Parameters + +| Name | Type | Description | +| --------------------------------- | ------------ | ------------------------------------------------------------------------------------------------ | +| `selected_metrics` | List | Metrics to be evaluated, such as lateral deviation, yaw deviation, and predicted path deviation. | +| `smoothing_window_size` | Integer | Determines the window size for smoothing path, should be an odd number. | +| `prediction_time_horizons` | list[double] | Time horizons for prediction evaluation in seconds. | +| `target_object.*.check_deviation` | bool | Whether to check deviation for specific object types (car, truck, etc.). | +| `debug_marker.*` | bool | Debugging parameters for marker visualization (history path, predicted path, etc.). | + +## Assumptions / Known limits + +It is assumed that the current positions of PredictedObjects are reasonably accurate. + +## Future extensions / Unimplemented parts + +- Increase rate in recognition per class +- Metrics for objects with strange physical behavior (e.g., going through a fence) +- Metrics for splitting objects +- Metrics for problems with objects that are normally stationary but move +- Disappearing object metrics diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp new file mode 100644 index 0000000000000..da7a23b6980b6 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -0,0 +1,53 @@ +// 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. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ + +#include "perception_online_evaluator/stat.hpp" + +#include +#include + +#include + +namespace perception_diagnostics +{ +namespace metrics +{ +using autoware_auto_perception_msgs::msg::PredictedPath; +using geometry_msgs::msg::Pose; + +/** + * @brief calculate lateral deviation of the given path from the reference path + * @param [in] ref_path reference path + * @param [in] pred_path predicted path + * @return calculated statistics + */ +double calcLateralDeviation(const std::vector & ref_path, const Pose & target_pose); + +/** + * @brief calculate yaw deviation of the given path from the reference path + * @param [in] ref_path reference path + * @param [in] pred_path predicted path + * @return calculated statistics + */ +double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose); + +std::vector calcPredictedPathDeviation( + const std::vector & ref_path, const PredictedPath & pred_path); +} // namespace metrics +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp new file mode 100644 index 0000000000000..8a2cddca476d4 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -0,0 +1,75 @@ +// 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. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ + +#include "perception_online_evaluator/stat.hpp" + +#include +#include +#include +#include + +namespace perception_diagnostics +{ +/** + * @brief Enumeration of trajectory metrics + */ +enum class Metric { + lateral_deviation, + yaw_deviation, + predicted_path_deviation, + SIZE, +}; + +using MetricStatMap = std::unordered_map>; + +static const std::unordered_map str_to_metric = { + {"lateral_deviation", Metric::lateral_deviation}, + {"yaw_deviation", Metric::yaw_deviation}, + {"predicted_path_deviation", Metric::predicted_path_deviation}}; + +static const std::unordered_map metric_to_str = { + {Metric::lateral_deviation, "lateral_deviation"}, + {Metric::yaw_deviation, "yaw_deviation"}, + {Metric::predicted_path_deviation, "predicted_path_deviation"}}; + +// Metrics descriptions +static const std::unordered_map metric_descriptions = { + {Metric::lateral_deviation, "Lateral_deviation[m]"}, + {Metric::yaw_deviation, "Yaw_deviation[rad]"}, + {Metric::predicted_path_deviation, "Predicted_path_deviation[m]"}}; + +namespace details +{ +static struct CheckCorrectMaps +{ + CheckCorrectMaps() + { + if ( + str_to_metric.size() != static_cast(Metric::SIZE) || + metric_to_str.size() != static_cast(Metric::SIZE) || + metric_descriptions.size() != static_cast(Metric::SIZE)) { + std::cerr << "[metrics/metrics.hpp] Maps are not defined for all metrics: "; + std::cerr << str_to_metric.size() << " " << metric_to_str.size() << " " + << metric_descriptions.size() << std::endl; + } + } +} check; + +} // namespace details +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp new file mode 100644 index 0000000000000..dd6756a17f194 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -0,0 +1,141 @@ +// 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. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ + +#include "perception_online_evaluator/metrics/deviation_metrics.hpp" +#include "perception_online_evaluator/metrics/metric.hpp" +#include "perception_online_evaluator/parameters.hpp" +#include "perception_online_evaluator/stat.hpp" + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include + +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using unique_identifier_msgs::msg::UUID; + +struct ObjectData +{ + PredictedObject object; + std::vector> path_pairs; + + std::vector getPredictedPath() const + { + std::vector path; + path.resize(path_pairs.size()); + std::transform( + path_pairs.begin(), path_pairs.end(), path.begin(), + [](const std::pair & pair) -> Pose { return pair.first; }); + return path; + } + + std::vector getHistoryPath() const + { + std::vector path; + path.resize(path_pairs.size()); + std::transform( + path_pairs.begin(), path_pairs.end(), path.begin(), + [](const std::pair & pair) -> Pose { return pair.second; }); + return path; + } +}; +using ObjectDataMap = std::unordered_map; + +// pair of history_path and smoothed_history_path for each object id +using HistoryPathMap = + std::unordered_map, std::vector>>; + +class MetricsCalculator +{ +public: + explicit MetricsCalculator(const std::shared_ptr & parameters) + : parameters_(parameters){}; + + /** + * @brief calculate + * @param [in] metric Metric enum value + * @return map of string describing the requested metric and the calculated value + */ + std::optional calculate(const Metric & metric) const; + + /** + * @brief set the dynamic objects used to calculate obstacle metrics + * @param [in] objects predicted objects + */ + void setPredictedObjects(const PredictedObjects & objects); + + HistoryPathMap getHistoryPathMap() const { return history_path_map_; } + ObjectDataMap getDebugObjectData() const { return debug_target_object_; } + +private: + std::shared_ptr parameters_; + + // Store predicted objects information and calculation results + std::unordered_map> object_map_; + HistoryPathMap history_path_map_; + + rclcpp::Time current_stamp_; + + // debug + mutable ObjectDataMap debug_target_object_; + + // Functions to calculate history path + void updateHistoryPath(); + std::vector averageFilterPath( + const std::vector & path, const size_t window_size) const; + std::vector generateHistoryPathWithPrev( + const std::vector & prev_history_path, const Pose & new_pose, const size_t window_size); + + // Update object data + void updateObjects( + const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object); + void deleteOldObjects(const rclcpp::Time stamp); + + // Calculate metrics + MetricStatMap calcLateralDeviationMetrics(const PredictedObjects & objects) const; + MetricStatMap calcYawDeviationMetrics(const PredictedObjects & objects) const; + MetricStatMap calcPredictedPathDeviationMetrics(const PredictedObjects & objects) const; + Stat calcPredictedPathDeviationMetrics( + const PredictedObjects & objects, const double time_horizon) const; + + bool hasPassedTime(const rclcpp::Time stamp) const; + bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; + double getTimeDelay() const; + + // Extract object + rclcpp::Time getClosestStamp(const rclcpp::Time stamp) const; + std::optional getObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const; + PredictedObjects getObjectsByStamp(const rclcpp::Time stamp) const; + +}; // class MetricsCalculator + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp new file mode 100644 index 0000000000000..98cd3c25b71a3 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp @@ -0,0 +1,58 @@ +// 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. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ + +#include "perception_online_evaluator/metrics/metric.hpp" + +#include +#include + +namespace perception_diagnostics +{ +/** + * @brief Enumeration of perception metrics + */ + +struct ObjectParameter +{ + bool check_deviation{false}; +}; + +struct DebugMarkerParameter +{ + bool show_history_path{false}; + bool show_history_path_arrows{false}; + bool show_smoothed_history_path{true}; + bool show_smoothed_history_path_arrows{false}; + bool show_predicted_path{true}; + bool show_predicted_path_gt{true}; + bool show_deviation_lines{true}; + bool show_object_polygon{true}; +}; + +struct Parameters +{ + std::vector metrics; + size_t smoothing_window_size{0}; + std::vector prediction_time_horizons; + DebugMarkerParameter debug_marker_parameters; + // parameters depend on object class + std::unordered_map object_parameters; +}; + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp new file mode 100644 index 0000000000000..b7535935ccd5f --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -0,0 +1,91 @@ +// 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. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ + +#include "perception_online_evaluator/metrics_calculator.hpp" +#include "perception_online_evaluator/parameters.hpp" +#include "perception_online_evaluator/stat.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using nav_msgs::msg::Odometry; + +using MarkerArray = visualization_msgs::msg::MarkerArray; + +/** + * @brief Node for perception evaluation + */ +class PerceptionOnlineEvaluatorNode : public rclcpp::Node +{ +public: + explicit PerceptionOnlineEvaluatorNode(const rclcpp::NodeOptions & node_options); + ~PerceptionOnlineEvaluatorNode() {} + + /** + * @brief callback on receiving a dynamic objects array + * @param [in] objects_msg received dynamic object array message + */ + void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); + + DiagnosticStatus generateDiagnosticStatus( + const std::string metric, const Stat & metric_stat) const; + +private: + // Subscribers and publishers + rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Publisher::SharedPtr metrics_pub_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // TF + std::shared_ptr transform_listener_{nullptr}; + std::unique_ptr tf_buffer_; + + // Parameters + std::shared_ptr parameters_; + void initParameter(); + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + + // Metrics Calculator + MetricsCalculator metrics_calculator_; + std::deque stamps_; + void publishMetrics(); + + // Debug + void publishDebugMarker(); +}; +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp new file mode 100644 index 0000000000000..63494f90d02ee --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/stat.hpp @@ -0,0 +1,93 @@ +// 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 +#include + +#ifndef PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ + +namespace perception_diagnostics +{ +/** + * @brief class to incrementally build statistics + * @typedef T type of the values (default to double) + */ +template +class Stat +{ +public: + /** + * @brief add a value + * @param value value to add + */ + void add(const T & value) + { + if (value < min_) { + min_ = value; + } + if (value > max_) { + max_ = value; + } + ++count_; + mean_ = mean_ + (value - mean_) / count_; + } + + /** + * @brief get the mean value + */ + long double mean() const { return mean_; } + + /** + * @brief get the minimum value + */ + T min() const { return min_; } + + /** + * @brief get the maximum value + */ + T max() const { return max_; } + + /** + * @brief get the number of values used to build this statistic + */ + unsigned int count() const { return count_; } + + template + friend std::ostream & operator<<(std::ostream & os, const Stat & stat); + +private: + T min_ = std::numeric_limits::max(); + T max_ = std::numeric_limits::min(); + long double mean_ = 0.0; + unsigned int count_ = 0; +}; + +/** + * @brief overload << operator for easy print to output stream + */ +template +std::ostream & operator<<(std::ostream & os, const Stat & stat) +{ + if (stat.count() == 0) { + os << "None None None"; + } else { + os << stat.min() << " " << stat.max() << " " << stat.mean(); + } + return os; +} + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__STAT_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp new file mode 100644 index 0000000000000..3ac4c1db9efbd --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -0,0 +1,82 @@ +// 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. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ + +#include + +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include +#include + +#include + +#include +#include +#include + +namespace marker_utils +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::Polygon2d; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +inline int64_t bitShift(int64_t original_id) +{ + return original_id << (sizeof(int32_t) * 8 / 2); +} + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info); + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b); +MarkerArray createPointsMarkerArray( + const std::vector poses, const std::string & ns, const int32_t id, const float r, + const float g, const float b); + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b); + +MarkerArray createPosesMarkerArray( + const std::vector poses, std::string && ns, const float & r, const float & g, + const float & b, const float & x_scale = 0.5, const float & y_scale = 0.2, + const float & z_scale = 0.2); + +std_msgs::msg::ColorRGBA createColorFromString(const std::string & str); + +MarkerArray createObjectPolygonMarkerArray( + const PredictedObject & object, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b); + +MarkerArray createDeviationLines( + const std::vector poses1, const std::vector poses2, const std::string & ns, + const float r, const float g, const float b); + +} // namespace marker_utils + +#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp new file mode 100644 index 0000000000000..5d4238f45dec9 --- /dev/null +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -0,0 +1,128 @@ +// 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. + +#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#define PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ + +#include "perception_online_evaluator/parameters.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +/** + * most of this file is copied from objects_filtering.hpp in safety_check of behavior_path_planner + */ + +namespace perception_diagnostics +{ + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; + +std::uint8_t getHighestProbLabel(const std::vector & classification); + +/** + * @brief Specifies which object class should be checked. + */ +struct ObjectTypesToCheck +{ + bool check_car{true}; ///< Check for cars. + bool check_truck{true}; ///< Check for trucks. + bool check_bus{true}; ///< Check for buses. + bool check_trailer{true}; ///< Check for trailers. + bool check_unknown{true}; ///< Check for unknown object types. + bool check_bicycle{true}; ///< Check for bicycles. + bool check_motorcycle{true}; ///< Check for motorcycles. + bool check_pedestrian{true}; ///< Check for pedestrians. +}; + +/** + * @brief Determines whether the predicted object type matches any of the target object types + * specified by the user. + * + * @param object The predicted object whose type is to be checked. + * @param target_object_types A structure containing boolean flags for each object type that the + * user is interested in checking. + * + * @return Returns true if the predicted object's highest probability label matches any of the + * specified target object types. + */ +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Filters objects in the 'selected' container based on the provided filter function. + * + * This function partitions the 'selected' container based on the 'filter' function + * and moves objects that satisfy the filter condition to the 'removed' container. + * + * @tparam Func The type of the filter function. + * @param selected [in,out] The container of objects to be filtered. + * @param removed [out] The container where objects not satisfying the filter condition are moved. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & selected, PredictedObjects & removed, Func filter) +{ + auto partitioned = std::partition(selected.objects.begin(), selected.objects.end(), filter); + removed.objects.insert(removed.objects.end(), partitioned, selected.objects.end()); + selected.objects.erase(partitioned, selected.objects.end()); +} + +/** + * @brief Filters objects in the 'objects' container based on the provided filter function. + * + * This function is an overload that simplifies filtering when you don't need to specify a + * separate 'removed' container. It internally creates a 'removed_objects' container and calls the + * main 'filterObjects' function. + * + * @tparam Func The type of the filter function. + * @param objects [in,out] The container of objects to be filtered. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & objects, Func filter) +{ + [[maybe_unused]] PredictedObjects removed_objects{}; + filterObjects(objects, removed_objects, filter); +} + +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param target_object_types The types of objects to retain after filtering. + */ +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types); + +/** + * @brief Filters the provided objects based on their classification. + * + * @param objects The predicted objects to be filtered. + * @param params The parameters for each object class. + */ +void filterDeviationCheckObjects( + PredictedObjects & objects, const std::unordered_map & params); + +} // namespace perception_diagnostics + +#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ diff --git a/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml new file mode 100644 index 0000000000000..08c4e11126ec1 --- /dev/null +++ b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml new file mode 100644 index 0000000000000..2ef179dbe3ff9 --- /dev/null +++ b/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml new file mode 100644 index 0000000000000..bc0f7ef82049b --- /dev/null +++ b/evaluator/perception_online_evaluator/package.xml @@ -0,0 +1,42 @@ + + + + perception_online_evaluator + 0.1.0 + ROS 2 node for evaluating perception + Fumiya Watanabe + Kosuke Takeuchi + Kotaro Uetake + Kyoichi Sugahara + Shunsuke Miura + Yoshi Ri + Apache License 2.0 + + Kosuke Takeuchi + + ament_cmake_auto + autoware_cmake + + autoware_perception_msgs + diagnostic_msgs + eigen + geometry_msgs + libgoogle-glog-dev + motion_utils + nav_msgs + pluginlib + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + vehicle_info_util + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml new file mode 100644 index 0000000000000..6749bac81aa35 --- /dev/null +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -0,0 +1,39 @@ +/**: + ros__parameters: + selected_metrics: + - lateral_deviation + - yaw_deviation + - predicted_path_deviation + + # this should be an odd number, because it includes the target point + smoothing_window_size: 11 + + prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] + + target_object: + car: + check_deviation: true + truck: + check_deviation: true + bus: + check_deviation: true + trailer: + check_deviation: true + bicycle: + check_deviation: true + motorcycle: + check_deviation: true + pedestrian: + check_deviation: true + unknown: + check_deviation: false + + debug_marker: + history_path: false + history_path_arrows: false + smoothed_history_path: true + smoothed_history_path_arrows: false + predicted_path: true + predicted_path_gt: true + deviation_lines: true + object_polygon: true diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp new file mode 100644 index 0000000000000..7a9435444c065 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -0,0 +1,65 @@ +// 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 "perception_online_evaluator/metrics/deviation_metrics.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" + +#include + +namespace perception_diagnostics +{ +namespace metrics +{ + +double calcLateralDeviation(const std::vector & ref_path, const Pose & target_pose) +{ + if (ref_path.empty()) { + return 0.0; + } + + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs( + tier4_autoware_utils::calcLateralDeviation(ref_path[nearest_index], target_pose.position)); +} + +double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose) +{ + if (ref_path.empty()) { + return 0.0; + } + + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, target_pose.position); + return std::abs(tier4_autoware_utils::calcYawDeviation(ref_path[nearest_index], target_pose)); +} + +std::vector calcPredictedPathDeviation( + const std::vector & ref_path, const PredictedPath & pred_path) +{ + std::vector deviations; + + if (ref_path.empty() || pred_path.path.empty()) { + return {}; + } + for (const Pose & p : pred_path.path) { + const size_t nearest_index = motion_utils::findNearestIndex(ref_path, p.position); + deviations.push_back( + tier4_autoware_utils::calcDistance2d(ref_path[nearest_index].position, p.position)); + } + + return deviations; +} +} // namespace metrics +} // namespace perception_diagnostics diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp new file mode 100644 index 0000000000000..bec6d84fcd7dd --- /dev/null +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -0,0 +1,458 @@ +// 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 "perception_online_evaluator/metrics_calculator.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" +#include "perception_online_evaluator/utils/objects_filtering.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +namespace perception_diagnostics +{ +std::optional MetricsCalculator::calculate(const Metric & metric) const +{ + if (object_map_.empty()) { + return {}; + } + + // time delay is max element of parameters_->prediction_time_horizons + const double time_delay = getTimeDelay(); + const auto target_stamp = current_stamp_ - rclcpp::Duration::from_seconds(time_delay); + if (!hasPassedTime(target_stamp)) { + return {}; + } + const auto target_objects = getObjectsByStamp(target_stamp); + + switch (metric) { + case Metric::lateral_deviation: + return calcLateralDeviationMetrics(target_objects); + case Metric::yaw_deviation: + return calcYawDeviationMetrics(target_objects); + case Metric::predicted_path_deviation: + return calcPredictedPathDeviationMetrics(target_objects); + default: + return {}; + } +} + +double MetricsCalculator::getTimeDelay() const +{ + const auto max_element_it = std::max_element( + parameters_->prediction_time_horizons.begin(), parameters_->prediction_time_horizons.end()); + if (max_element_it != parameters_->prediction_time_horizons.end()) { + return *max_element_it; + } + throw std::runtime_error("prediction_time_horizons is empty"); +} + +bool MetricsCalculator::hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const +{ + if (object_map_.find(uuid) == object_map_.end()) { + return false; + } + const auto oldest_stamp = object_map_.at(uuid).begin()->first; + return oldest_stamp <= stamp; +} + +bool MetricsCalculator::hasPassedTime(const rclcpp::Time stamp) const +{ + std::vector timestamps; + for (const auto & [uuid, stamp_and_objects] : object_map_) { + if (!stamp_and_objects.empty()) { + timestamps.push_back(stamp_and_objects.begin()->first); + } + } + + auto it = std::min_element(timestamps.begin(), timestamps.end()); + if (it != timestamps.end()) { + rclcpp::Time oldest_stamp = *it; + if (oldest_stamp > stamp) { + return false; + } + } + return true; +} + +rclcpp::Time MetricsCalculator::getClosestStamp(const rclcpp::Time stamp) const +{ + rclcpp::Time closest_stamp; + rclcpp::Duration min_duration = + rclcpp::Duration::from_nanoseconds(std::numeric_limits::max()); + + for (const auto & [uuid, stamp_and_objects] : object_map_) { + if (!stamp_and_objects.empty()) { + auto it = std::lower_bound( + stamp_and_objects.begin(), stamp_and_objects.end(), stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + // check the upper bound + if (it != stamp_and_objects.end()) { + const auto duration = it->first - stamp; + if (std::abs(duration.nanoseconds()) < min_duration.nanoseconds()) { + min_duration = duration; + closest_stamp = it->first; + } + } + + // check the lower bound (if it is not the first element) + if (it != stamp_and_objects.begin()) { + const auto prev_it = std::prev(it); + const auto duration = stamp - prev_it->first; + if (std::abs(duration.nanoseconds()) < min_duration.nanoseconds()) { + min_duration = duration; + closest_stamp = prev_it->first; + } + } + } + } + + return closest_stamp; +} + +std::optional MetricsCalculator::getObjectByStamp( + const std::string uuid, const rclcpp::Time stamp) const +{ + const auto closest_stamp = getClosestStamp(stamp); + auto it = std::lower_bound( + object_map_.at(uuid).begin(), object_map_.at(uuid).end(), closest_stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + if (it != object_map_.at(uuid).end() && it->first == closest_stamp) { + return it->second; + } + return std::nullopt; +} + +PredictedObjects MetricsCalculator::getObjectsByStamp(const rclcpp::Time stamp) const +{ + const auto closest_stamp = getClosestStamp(stamp); + + PredictedObjects objects; + objects.header.stamp = stamp; + for (const auto & [uuid, stamp_and_objects] : object_map_) { + auto it = std::lower_bound( + stamp_and_objects.begin(), stamp_and_objects.end(), closest_stamp, + [](const auto & pair, const rclcpp::Time & val) { return pair.first < val; }); + + // add the object only if the element pointed to by lower_bound is equal to stamp + if (it != stamp_and_objects.end() && it->first == closest_stamp) { + objects.objects.push_back(it->second); + } + } + return objects; +} + +MetricStatMap MetricsCalculator::calcLateralDeviationMetrics(const PredictedObjects & objects) const +{ + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcLateralDeviation(history_path, object_pose)); + } + + MetricStatMap metric_stat_map; + metric_stat_map["lateral_deviation"] = stat; + return metric_stat_map; +} + +MetricStatMap MetricsCalculator::calcYawDeviationMetrics(const PredictedObjects & objects) const +{ + Stat stat{}; + const auto stamp = rclcpp::Time(objects.header.stamp); + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + if (!hasPassedTime(uuid, stamp)) { + continue; + } + const auto object_pose = object.kinematics.initial_pose_with_covariance.pose; + const auto history_path = history_path_map_.at(uuid).second; + if (history_path.empty()) { + continue; + } + stat.add(metrics::calcYawDeviation(history_path, object_pose)); + } + + MetricStatMap metric_stat_map; + metric_stat_map["yaw_deviation"] = stat; + return metric_stat_map; +} + +MetricStatMap MetricsCalculator::calcPredictedPathDeviationMetrics( + const PredictedObjects & objects) const +{ + const auto time_horizons = parameters_->prediction_time_horizons; + + MetricStatMap metric_stat_map; + for (const double time_horizon : time_horizons) { + const auto stat = calcPredictedPathDeviationMetrics(objects, time_horizon); + std::ostringstream stream; + stream << std::fixed << std::setprecision(2) << time_horizon; + std::string time_horizon_str = stream.str(); + metric_stat_map["predicted_path_deviation_" + time_horizon_str] = stat; + } + + return metric_stat_map; +} + +Stat MetricsCalculator::calcPredictedPathDeviationMetrics( + const PredictedObjects & objects, const double time_horizon) const +{ + // For each object, select the predicted path that is closest to the history path and store the + // distance to the history path + std::unordered_map>> + deviation_map_for_paths; + // For debugging. Save the pairs of predicted path pose and history path pose. + // Visualize the correspondence in rviz from the node. + std::unordered_map>> + debug_predicted_path_pairs_map; + + // Find the corresponding pose in the history path for each pose of the predicted path of each + // object, and record the distances + const auto stamp = objects.header.stamp; + for (const auto & object : objects.objects) { + const auto uuid = tier4_autoware_utils::toHexString(object.object_id); + const auto predicted_paths = object.kinematics.predicted_paths; + for (size_t i = 0; i < predicted_paths.size(); i++) { + const auto predicted_path = predicted_paths[i]; + const std::string path_id = uuid + "_" + std::to_string(i); + for (size_t j = 0; j < predicted_path.path.size(); j++) { + const double time_duration = + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(j); + if (time_duration > time_horizon) { + break; + } + const rclcpp::Time target_stamp = + rclcpp::Time(stamp) + rclcpp::Duration::from_seconds(time_duration); + if (!hasPassedTime(uuid, target_stamp)) { + continue; + } + const auto history_object_opt = getObjectByStamp(uuid, target_stamp); + if (!history_object_opt.has_value()) { + continue; + } + const auto history_object = history_object_opt.value(); + const auto history_pose = history_object.kinematics.initial_pose_with_covariance.pose; + const Pose & p = predicted_path.path[j]; + const double distance = + tier4_autoware_utils::calcDistance2d(p.position, history_pose.position); + deviation_map_for_paths[uuid][i].push_back(distance); + // debug + debug_predicted_path_pairs_map[path_id].push_back(std::make_pair(p, history_pose)); + } + } + } + + // Select the predicted path with the smallest deviation for each object + std::unordered_map> deviation_map_for_objects; + for (const auto & [uuid, deviation_map] : deviation_map_for_paths) { + size_t min_deviation_index = 0; + double min_sum_deviation = std::numeric_limits::max(); + for (const auto & [i, deviations] : deviation_map) { + if (deviations.empty()) { + continue; + } + const double sum = std::accumulate(deviations.begin(), deviations.end(), 0.0); + if (sum < min_sum_deviation) { + min_sum_deviation = sum; + min_deviation_index = i; + } + } + deviation_map_for_objects[uuid] = deviation_map.at(min_deviation_index); + + // debug: save the delayed target object and the corresponding predicted path + const auto path_id = uuid + "_" + std::to_string(min_deviation_index); + const auto target_stamp_object = getObjectByStamp(uuid, stamp); + if (target_stamp_object.has_value()) { + ObjectData object_data; + object_data.object = target_stamp_object.value(); + object_data.path_pairs = debug_predicted_path_pairs_map[path_id]; + debug_target_object_[uuid] = object_data; + } + } + + // Store the deviation as a metric + Stat stat; + for (const auto & [uuid, deviations] : deviation_map_for_objects) { + if (deviations.empty()) { + continue; + } + for (const auto & deviation : deviations) { + stat.add(deviation); + } + } + return stat; +} + +void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) +{ + // using TimeStamp = builtin_interfaces::msg::Time; + current_stamp_ = objects.header.stamp; + + // store objects to check deviation + { + auto deviation_check_objects = objects; + filterDeviationCheckObjects(deviation_check_objects, parameters_->object_parameters); + using tier4_autoware_utils::toHexString; + for (const auto & object : deviation_check_objects.objects) { + std::string uuid = toHexString(object.object_id); + updateObjects(uuid, current_stamp_, object); + } + deleteOldObjects(current_stamp_); + updateHistoryPath(); + } +} + +void MetricsCalculator::deleteOldObjects(const rclcpp::Time stamp) +{ + // delete the data older than 2*time_delay_ + const double time_delay = getTimeDelay(); + for (auto & [uuid, stamp_and_objects] : object_map_) { + for (auto it = stamp_and_objects.begin(); it != stamp_and_objects.end();) { + if (it->first < stamp - rclcpp::Duration::from_seconds(time_delay * 2)) { + it = stamp_and_objects.erase(it); + } else { + break; + } + } + } + + const auto object_map = object_map_; + for (const auto & [uuid, stamp_and_objects] : object_map) { + if (stamp_and_objects.empty()) { + object_map_.erase(uuid); + history_path_map_.erase(uuid); + debug_target_object_.erase(uuid); // debug + } + } +} + +void MetricsCalculator::updateObjects( + const std::string uuid, const rclcpp::Time stamp, const PredictedObject & object) +{ + object_map_[uuid][stamp] = object; +} + +void MetricsCalculator::updateHistoryPath() +{ + const double window_size = parameters_->smoothing_window_size; + + for (const auto & [uuid, stamp_and_objects] : object_map_) { + std::vector history_path; + for (const auto & [stamp, object] : stamp_and_objects) { + history_path.push_back(object.kinematics.initial_pose_with_covariance.pose); + } + + // pair of history_path(raw) and smoothed_history_path + // history_path(raw) is just for debugging + history_path_map_[uuid] = + std::make_pair(history_path, averageFilterPath(history_path, window_size)); + } +} + +std::vector MetricsCalculator::generateHistoryPathWithPrev( + const std::vector & prev_history_path, const Pose & new_pose, const size_t window_size) +{ + std::vector history_path; + const size_t half_window_size = static_cast(window_size / 2); + history_path.insert( + history_path.end(), prev_history_path.begin(), prev_history_path.end() - half_window_size); + + std::vector updated_poses; + updated_poses.insert( + updated_poses.end(), prev_history_path.end() - half_window_size * 2, prev_history_path.end()); + updated_poses.push_back(new_pose); + + updated_poses = averageFilterPath(updated_poses, window_size); + history_path.insert( + history_path.end(), updated_poses.begin() + half_window_size, updated_poses.end()); + return history_path; +} + +std::vector MetricsCalculator::averageFilterPath( + const std::vector & path, const size_t window_size) const +{ + using tier4_autoware_utils::calcAzimuthAngle; + using tier4_autoware_utils::calcDistance2d; + using tier4_autoware_utils::createQuaternionFromYaw; + + std::vector filtered_path; + filtered_path.reserve(path.size()); // Reserve space to avoid reallocations + + const size_t half_window = static_cast(window_size / 2); + + // Calculate the moving average for positions + for (size_t i = 0; i < path.size(); ++i) { + double sum_x = 0.0, sum_y = 0.0, sum_z = 0.0; + size_t valid_points = 0; // Correctly initialize and use as counter + + for (size_t j = std::max(i - half_window, static_cast(0)); + j <= std::min(i + half_window, path.size() - 1); ++j) { + sum_x += path[j].position.x; + sum_y += path[j].position.y; + sum_z += path[j].position.z; + ++valid_points; + } + + Pose average_pose; + if (valid_points > 0) { // Prevent division by zero + average_pose.position.x = sum_x / valid_points; + average_pose.position.y = sum_y / valid_points; + average_pose.position.z = sum_z / valid_points; + } else { + average_pose.position = path.at(i).position; + } + + // Placeholder for orientation to ensure structure integrity + average_pose.orientation = path.at(i).orientation; + filtered_path.push_back(average_pose); + } + + // Calculate yaw and convert to quaternion after averaging positions + for (size_t i = 0; i < filtered_path.size(); ++i) { + Pose & p = filtered_path[i]; + + // if the current pose is too close to the previous one, use the previous orientation + if (i > 0) { + const Pose & p_prev = filtered_path[i - 1]; + if (calcDistance2d(p_prev.position, p.position) < 0.1) { + p.orientation = p_prev.orientation; + continue; + } + } + + if (i < filtered_path.size() - 1) { + const double yaw = calcAzimuthAngle(p.position, filtered_path[i + 1].position); + filtered_path[i].orientation = createQuaternionFromYaw(yaw); + } else if (filtered_path.size() > 1) { + // For the last point, use the orientation of the second-to-last point + p.orientation = filtered_path[i - 1].orientation; + } + } + + return filtered_path; +} + +} // namespace perception_diagnostics diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp new file mode 100644 index 0000000000000..cf98ccc4c5460 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -0,0 +1,337 @@ +// 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 "perception_online_evaluator/perception_online_evaluator_node.hpp" + +#include "perception_online_evaluator/utils/marker_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" + +#include + +#include "boost/lexical_cast.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace perception_diagnostics +{ +PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( + const rclcpp::NodeOptions & node_options) +: Node("perception_online_evaluator", node_options), + parameters_(std::make_shared()), + metrics_calculator_(parameters_) +{ + using std::placeholders::_1; + + google::InitGoogleLogging("perception_online_evaluator_node"); + google::InstallFailureSignalHandler(); + + objects_sub_ = create_subscription( + "~/input/objects", 1, std::bind(&PerceptionOnlineEvaluatorNode::onObjects, this, _1)); + metrics_pub_ = create_publisher("~/metrics", 1); + pub_marker_ = create_publisher("~/markers", 10); + + tf_buffer_ = std::make_unique(this->get_clock()); + transform_listener_ = std::make_shared(*tf_buffer_); + + // Parameters + initParameter(); + + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&PerceptionOnlineEvaluatorNode::onParameter, this, std::placeholders::_1)); +} + +void PerceptionOnlineEvaluatorNode::publishMetrics() +{ + DiagnosticArray metrics_msg; + + // calculate metrics + for (const Metric & metric : parameters_->metrics) { + const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); + if (!metric_stat_map.has_value()) { + continue; + } + + for (const auto & [metric, stat] : metric_stat_map.value()) { + if (stat.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); + } + } + } + + // publish metrics + if (!metrics_msg.status.empty()) { + metrics_msg.header.stamp = now(); + metrics_pub_->publish(metrics_msg); + } + publishDebugMarker(); +} + +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string metric, const Stat & metric_stat) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "min"; + key_value.value = std::to_string(metric_stat.min()); + status.values.push_back(key_value); + key_value.key = "max"; + key_value.value = std::to_string(metric_stat.max()); + status.values.push_back(key_value); + key_value.key = "mean"; + key_value.value = std::to_string(metric_stat.mean()); + status.values.push_back(key_value); + + return status; +} + +void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) +{ + metrics_calculator_.setPredictedObjects(*objects_msg); + publishMetrics(); +} + +void PerceptionOnlineEvaluatorNode::publishDebugMarker() +{ + using marker_utils::createColorFromString; + using marker_utils::createDeviationLines; + using marker_utils::createObjectPolygonMarkerArray; + using marker_utils::createPointsMarkerArray; + using marker_utils::createPosesMarkerArray; + + MarkerArray marker; + + const auto add = [&marker](MarkerArray added) { + for (auto & marker : added.markers) { + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + } + tier4_autoware_utils::appendMarkerArray(added, &marker); + }; + + const auto history_path_map = metrics_calculator_.getHistoryPathMap(); + const auto & p = parameters_->debug_marker_parameters; + + for (const auto & [uuid, history_path] : history_path_map) { + { + const auto c = createColorFromString(uuid + "_raw"); + if (p.show_history_path) { + add(createPointsMarkerArray(history_path.first, "history_path_" + uuid, 0, c.r, c.g, c.b)); + } + if (p.show_history_path_arrows) { + add(createPosesMarkerArray( + history_path.first, "history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, 0.05)); + } + } + { + const auto c = createColorFromString(uuid); + if (p.show_smoothed_history_path) { + add(createPointsMarkerArray( + history_path.second, "smoothed_history_path_" + uuid, 0, c.r, c.g, c.b)); + } + if (p.show_smoothed_history_path_arrows) { + add(createPosesMarkerArray( + history_path.second, "smoothed_history_path_arrows_" + uuid, c.r, c.g, c.b, 0.1, 0.05, + 0.05)); + } + } + } + const auto object_data_map = metrics_calculator_.getDebugObjectData(); + for (const auto & [uuid, object_data] : object_data_map) { + const auto c = createColorFromString(uuid); + const auto predicted_path = object_data.getPredictedPath(); + const auto history_path = object_data.getHistoryPath(); + if (p.show_predicted_path) { + add(createPosesMarkerArray(predicted_path, "predicted_path_" + uuid, 0, 0, 1)); + } + if (p.show_predicted_path_gt) { + add(createPosesMarkerArray(history_path, "predicted_path_gt_" + uuid, 1, 0, 0)); + } + if (p.show_deviation_lines) { + add(createDeviationLines(predicted_path, history_path, "deviation_lines_" + uuid, 1, 1, 1)); + } + if (p.show_object_polygon) { + add(createObjectPolygonMarkerArray( + object_data.object, "object_polygon_" + uuid, 0, c.r, c.g, c.b)); + } + } + + pub_marker_->publish(marker); +} + +rcl_interfaces::msg::SetParametersResult PerceptionOnlineEvaluatorNode::onParameter( + const std::vector & parameters) +{ + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + updateParam(parameters, "smoothing_window_size", p->smoothing_window_size); + + // update metrics + { + std::vector metrics_str; + updateParam>(parameters, "selected_metrics", metrics_str); + std::vector metrics; + for (const std::string & selected_metric : metrics_str) { + const Metric metric = str_to_metric.at(selected_metric); + metrics.push_back(metric); + } + p->metrics = metrics; + } + + // update parameters for each object class + { + const auto get_object_param = [&](std::string && ns) -> std::optional { + ObjectParameter param{}; + if (updateParam(parameters, ns + "check_deviation", param.check_deviation)) { + return param; + } + return std::nullopt; + }; + + const std::string ns = "target_object."; + if (const auto new_param = get_object_param(ns + "car.")) { + p->object_parameters.at(ObjectClassification::CAR) = *new_param; + } + if (const auto new_param = get_object_param(ns + "truck.")) { + p->object_parameters.at(ObjectClassification::TRUCK) = *new_param; + } + if (const auto new_param = get_object_param(ns + "bus.")) { + p->object_parameters.at(ObjectClassification::BUS) = *new_param; + } + if (const auto new_param = get_object_param(ns + "trailer.")) { + p->object_parameters.at(ObjectClassification::TRAILER) = *new_param; + } + if (const auto new_param = get_object_param(ns + "bicycle.")) { + p->object_parameters.at(ObjectClassification::BICYCLE) = *new_param; + } + if (const auto new_param = get_object_param(ns + "motorcycle.")) { + p->object_parameters.at(ObjectClassification::MOTORCYCLE) = *new_param; + } + if (const auto new_param = get_object_param(ns + "pedestrian.")) { + p->object_parameters.at(ObjectClassification::PEDESTRIAN) = *new_param; + } + if (const auto new_param = get_object_param(ns + "unknown.")) { + p->object_parameters.at(ObjectClassification::UNKNOWN) = *new_param; + } + } + + // update debug marker parameters + { + const std::string ns = "debug_marker."; + updateParam( + parameters, ns + "history_path", p->debug_marker_parameters.show_history_path); + updateParam( + parameters, ns + "history_path_arrows", p->debug_marker_parameters.show_history_path_arrows); + updateParam( + parameters, ns + "smoothed_history_path", + p->debug_marker_parameters.show_smoothed_history_path); + updateParam( + parameters, ns + "smoothed_history_path_arrows", + p->debug_marker_parameters.show_smoothed_history_path_arrows); + updateParam( + parameters, ns + "predicted_path", p->debug_marker_parameters.show_predicted_path); + updateParam( + parameters, ns + "predicted_path_gt", p->debug_marker_parameters.show_predicted_path_gt); + updateParam( + parameters, ns + "deviation_lines", p->debug_marker_parameters.show_deviation_lines); + updateParam( + parameters, ns + "object_polygon", p->debug_marker_parameters.show_object_polygon); + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + return result; +} + +void PerceptionOnlineEvaluatorNode::initParameter() +{ + using tier4_autoware_utils::getOrDeclareParameter; + using tier4_autoware_utils::updateParam; + + auto & p = parameters_; + + p->smoothing_window_size = getOrDeclareParameter(*this, "smoothing_window_size"); + p->prediction_time_horizons = + getOrDeclareParameter>(*this, "prediction_time_horizons"); + + // set metrics + const auto selected_metrics = + getOrDeclareParameter>(*this, "selected_metrics"); + for (const std::string & selected_metric : selected_metrics) { + const Metric metric = str_to_metric.at(selected_metric); + parameters_->metrics.push_back(metric); + } + + // set parameters for each object class + { + const auto get_object_param = [&](std::string && ns) -> ObjectParameter { + ObjectParameter param{}; + param.check_deviation = getOrDeclareParameter(*this, ns + "check_deviation"); + return param; + }; + + const std::string ns = "target_object."; + p->object_parameters.emplace(ObjectClassification::CAR, get_object_param(ns + "car.")); + p->object_parameters.emplace(ObjectClassification::TRUCK, get_object_param(ns + "truck.")); + p->object_parameters.emplace(ObjectClassification::BUS, get_object_param(ns + "bus.")); + p->object_parameters.emplace(ObjectClassification::TRAILER, get_object_param(ns + "trailer.")); + p->object_parameters.emplace(ObjectClassification::BICYCLE, get_object_param(ns + "bicycle.")); + p->object_parameters.emplace( + ObjectClassification::MOTORCYCLE, get_object_param(ns + "motorcycle.")); + p->object_parameters.emplace( + ObjectClassification::PEDESTRIAN, get_object_param(ns + "pedestrian.")); + p->object_parameters.emplace(ObjectClassification::UNKNOWN, get_object_param(ns + "unknown.")); + } + + // set debug marker parameters + { + const std::string ns = "debug_marker."; + p->debug_marker_parameters.show_history_path = + getOrDeclareParameter(*this, ns + "history_path"); + p->debug_marker_parameters.show_history_path_arrows = + getOrDeclareParameter(*this, ns + "history_path_arrows"); + p->debug_marker_parameters.show_smoothed_history_path = + getOrDeclareParameter(*this, ns + "smoothed_history_path"); + p->debug_marker_parameters.show_smoothed_history_path_arrows = + getOrDeclareParameter(*this, ns + "smoothed_history_path_arrows"); + p->debug_marker_parameters.show_predicted_path = + getOrDeclareParameter(*this, ns + "predicted_path"); + p->debug_marker_parameters.show_predicted_path_gt = + getOrDeclareParameter(*this, ns + "predicted_path_gt"); + p->debug_marker_parameters.show_deviation_lines = + getOrDeclareParameter(*this, ns + "deviation_lines"); + p->debug_marker_parameters.show_object_polygon = + getOrDeclareParameter(*this, ns + "object_polygon"); + } +} +} // namespace perception_diagnostics + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(perception_diagnostics::PerceptionOnlineEvaluatorNode) diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp new file mode 100644 index 0000000000000..75af92e83dcd8 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -0,0 +1,196 @@ +// 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 "perception_online_evaluator/utils/marker_utils.hpp" + +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include +#include +#include + +#include + +namespace marker_utils +{ +using std_msgs::msg::ColorRGBA; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; +using visualization_msgs::msg::Marker; + +void addFootprintMarker( + visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, + const vehicle_info_util::VehicleInfo & vehicle_info) +{ + const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; + const double base_to_rear = vehicle_info.rear_overhang_m; + + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position); + marker.points.push_back( + tier4_autoware_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position); + marker.points.push_back(marker.points.front()); +} + +MarkerArray createFootprintMarkerArray( + const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) +{ + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", current_time, ns, id, Marker::LINE_STRIP, createMarkerScale(0.2, 0.2, 0.2), + createMarkerColor(r, g, b, 0.999)); + marker.lifetime = rclcpp::Duration::from_seconds(1.5); + + MarkerArray marker_array; + + addFootprintMarker(marker, base_link_pose, vehicle_info); + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector points, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & point : points) { + marker.points.push_back(point); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createPointsMarkerArray( + const std::vector poses, const std::string & ns, const int32_t id, const float r, + const float g, const float b) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + for (const auto & pose : poses) { + marker.points.push_back(pose.position); + } + + MarkerArray msg; + msg.markers.push_back(marker); + return msg; +} + +MarkerArray createDeviationLines( + const std::vector poses1, const std::vector poses2, const std::string & ns, + const float r, const float g, const float b) +{ + MarkerArray msg; + + const size_t max_idx = std::min(poses1.size(), poses2.size()); + for (size_t i = 0; i < max_idx; ++i) { + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::LINE_STRIP, + createMarkerScale(0.005, 0.0, 0.0), createMarkerColor(r, g, b, 0.5)); + marker.points.push_back(poses1.at(i).position); + marker.points.push_back(poses2.at(i).position); + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createPoseMarkerArray( + const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, + const float & b) +{ + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::ARROW, + createMarkerScale(0.7, 0.3, 0.3), createMarkerColor(r, g, b, 0.999)); + marker.pose = pose; + msg.markers.push_back(marker); + + return msg; +} + +MarkerArray createPosesMarkerArray( + const std::vector poses, std::string && ns, const float & r, const float & g, + const float & b, const float & x_scale, const float & y_scale, const float & z_scale) +{ + MarkerArray msg; + + for (size_t i = 0; i < poses.size(); ++i) { + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, i, Marker::ARROW, + createMarkerScale(x_scale, y_scale, z_scale), createMarkerColor(r, g, b, 0.5)); + marker.pose = poses.at(i); + msg.markers.push_back(marker); + } + + return msg; +} + +std_msgs::msg::ColorRGBA createColorFromString(const std::string & str) +{ + const auto hash = std::hash{}(str); + const auto r = (hash & 0xFF) / 255.0; + const auto g = ((hash >> 8) & 0xFF) / 255.0; + const auto b = ((hash >> 16) & 0xFF) / 255.0; + return tier4_autoware_utils::createMarkerColor(r, g, b, 0.5); +} + +MarkerArray createObjectPolygonMarkerArray( + const PredictedObject & object, std::string && ns, const int32_t & id, const float & r, + const float & g, const float & b) +{ + MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(r, g, b, 0.999)); + + const double z = object.kinematics.initial_pose_with_covariance.pose.position.z; + const double height = object.shape.dimensions.z; + const auto polygon = tier4_autoware_utils::toPolygon2d( + object.kinematics.initial_pose_with_covariance.pose, object.shape); + for (const auto & p : polygon.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), z - height / 2)); + marker.points.push_back(createPoint(p.x(), p.y(), z + height / 2)); + } + marker.id = id; + msg.markers.push_back(marker); + + return msg; +} + +} // namespace marker_utils diff --git a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp new file mode 100644 index 0000000000000..97daee36175f3 --- /dev/null +++ b/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp @@ -0,0 +1,103 @@ +// 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 "perception_online_evaluator/utils/objects_filtering.hpp" + +namespace perception_diagnostics +{ +ObjectTypesToCheck getDeviationCheckObjectTypes( + const std::unordered_map & params) +{ + ObjectTypesToCheck object_types_to_check; + for (const auto & [object_class, object_param] : params) { + switch (object_class) { + case ObjectClassification::CAR: + object_types_to_check.check_car = object_param.check_deviation; + break; + case ObjectClassification::TRUCK: + object_types_to_check.check_truck = object_param.check_deviation; + break; + case ObjectClassification::BUS: + object_types_to_check.check_bus = object_param.check_deviation; + break; + case ObjectClassification::TRAILER: + object_types_to_check.check_trailer = object_param.check_deviation; + break; + case ObjectClassification::BICYCLE: + object_types_to_check.check_bicycle = object_param.check_deviation; + break; + case ObjectClassification::MOTORCYCLE: + object_types_to_check.check_motorcycle = object_param.check_deviation; + break; + case ObjectClassification::PEDESTRIAN: + object_types_to_check.check_pedestrian = object_param.check_deviation; + break; + case ObjectClassification::UNKNOWN: + object_types_to_check.check_unknown = object_param.check_deviation; + break; + default: + break; + } + } + return object_types_to_check; +} + +std::uint8_t getHighestProbLabel(const std::vector & classification) +{ + std::uint8_t label = ObjectClassification::UNKNOWN; + float highest_prob = 0.0; + for (const auto & _class : classification) { + if (highest_prob < _class.probability) { + highest_prob = _class.probability; + label = _class.label; + } + } + + return label; +} + +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types) +{ + const auto t = getHighestProbLabel(object.classification); + + return ( + (t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); +} + +void filterObjectsByClass( + PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) +{ + const auto filter = [&](const auto & object) { + return isTargetObjectType(object, target_object_types); + }; + + filterObjects(objects, filter); +} + +void filterDeviationCheckObjects( + PredictedObjects & objects, const std::unordered_map & params) +{ + const auto object_types = getDeviationCheckObjectTypes(params); + filterObjectsByClass(objects, object_types); +} + +} // namespace perception_diagnostics diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp new file mode 100644 index 0000000000000..095fb130426f0 --- /dev/null +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -0,0 +1,521 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +#include "boost/lexical_cast.hpp" + +#include +#include + +#include +#include +#include +#include + +using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; +using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; +using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; +using MarkerArray = visualization_msgs::msg::MarkerArray; +using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; + +using tier4_autoware_utils::generateUUID; + +constexpr double epsilon = 1e-6; + +class EvalTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + + rclcpp::NodeOptions options; + const auto share_dir = + ament_index_cpp::get_package_share_directory("perception_online_evaluator"); + options.arguments( + {"--ros-args", "--params-file", + share_dir + "/param/perception_online_evaluator.defaults.yaml"}); + options.append_parameter_override("prediction_time_horizons", std::vector{5.0}); + options.append_parameter_override("smoothing_window_size", 11); + + dummy_node = std::make_shared("perception_online_evaluator_test", options); + eval_node = std::make_shared(options); + // Enable all logging in the node + auto ret = rcutils_logging_set_logger_level( + dummy_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + ret = rcutils_logging_set_logger_level( + eval_node->get_logger().get_name(), RCUTILS_LOG_SEVERITY_DEBUG); + if (ret != RCUTILS_RET_OK) { + std::cerr << "Failed to set logging severity to DEBUG\n"; + } + objects_pub_ = rclcpp::create_publisher( + dummy_node, "/perception_online_evaluator/input/objects", 1); + + marker_sub_ = rclcpp::create_subscription( + eval_node, "perception_online_evaluator/markers", 10, + [this]([[maybe_unused]] const MarkerArray::SharedPtr msg) { has_received_marker_ = true; }); + uuid_ = generateUUID(); + } + + ~EvalTest() override + { + rclcpp::shutdown(); + google::ShutdownGoogleLogging(); + } + + void setTargetMetric(perception_diagnostics::Metric metric) + { + const auto metric_str = perception_diagnostics::metric_to_str.at(metric); + setTargetMetric(metric_str); + } + + void setTargetMetric(std::string metric_str) + { + const auto is_target_metric = [metric_str](const auto & status) { + return status.name == metric_str; + }; + metric_sub_ = rclcpp::create_subscription( + eval_node, "/perception_online_evaluator/metrics", 1, + [=](const DiagnosticArray::ConstSharedPtr msg) { + const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); + if (it != msg->status.end()) { + std::cerr << it->values[0].key << " " << it->values[0].value << " " << it->values[1].key + << " " << it->values[1].value << " " << it->values[2].key << " " + << it->values[2].value << std::endl; + metric_value_ = boost::lexical_cast(it->values[2].value); + metric_updated_ = true; + } + }); + } + + PredictedObject makePredictedObject(const std::vector> & predicted_path) + { + PredictedObject object; + object.object_id = uuid_; + ObjectClassification classification; + classification.label = ObjectClassification::CAR; + classification.probability = 1.0; + + object.classification = {classification}; + + object.kinematics.initial_pose_with_covariance.pose.position.x = predicted_path.front().first; + object.kinematics.initial_pose_with_covariance.pose.position.y = predicted_path.front().second; + object.kinematics.initial_pose_with_covariance.pose.position.z = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.x = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.y = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0.0; + object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1.0; + + autoware_auto_perception_msgs::msg::PredictedPath path; + for (size_t i = 0; i < predicted_path.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = predicted_path[i].first; + pose.position.y = predicted_path[i].second; + pose.position.z = 0.0; + pose.orientation.x = 0.0; + pose.orientation.y = 0.0; + pose.orientation.z = 0.0; + pose.orientation.w = 1.0; + path.path.push_back(pose); + } + + path.confidence = 1.0; + path.time_step = rclcpp::Duration::from_seconds(time_step_); + object.kinematics.predicted_paths.push_back(path); + + return object; + } + + PredictedObjects makePredictedObjects( + const std::vector> & predicted_path) + { + PredictedObjects objects; + objects.objects.push_back(makePredictedObject(predicted_path)); + objects.header.stamp = rclcpp::Time(0); + return objects; + } + + PredictedObjects makeStraightPredictedObjects(const double time) + { + std::vector> predicted_path; + for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { + predicted_path.push_back({velocity_ * (time + i * time_step_), 0.0}); + } + auto objects = makePredictedObjects(predicted_path); + objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); + return objects; + } + + PredictedObjects makeDeviatedStraightPredictedObjects(const double time, const double deviation) + { + std::vector> predicted_path; + for (size_t i = 0; i <= time_horizon_ / time_step_; i++) { + predicted_path.push_back({velocity_ * (time + i * time_step_), deviation}); + } + auto objects = makePredictedObjects(predicted_path); + objects.header.stamp = rclcpp::Time(0) + rclcpp::Duration::from_seconds(time); + return objects; + } + + PredictedObjects rotateObjects(const PredictedObjects objects, const double yaw) + { + PredictedObjects rotated_objects = objects; + for (auto & object : rotated_objects.objects) { + object.kinematics.initial_pose_with_covariance.pose.orientation.z = sin(yaw / 2); + object.kinematics.initial_pose_with_covariance.pose.orientation.w = cos(yaw / 2); + } + return rotated_objects; + } + + double publishObjectsAndGetMetric(const PredictedObjects & objects) + { + metric_updated_ = false; + objects_pub_->publish(objects); + const auto now = rclcpp::Clock().now(); + while (!metric_updated_) { + rclcpp::spin_some(dummy_node); + rclcpp::spin_some(eval_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + // timeout + if (rclcpp::Clock().now() - now > rclcpp::Duration::from_seconds(5)) { + throw std::runtime_error("Timeout while waiting for metric update"); + } + } + return metric_value_; + } + + void publishObjects(const PredictedObjects & objects) + { + objects_pub_->publish(objects); + rclcpp::spin_some(eval_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(dummy_node); + } + + void waitForDummyNode() + { + // wait for the marker to be published + publishObjects(makeStraightPredictedObjects(0)); + while (!has_received_marker_) { + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + rclcpp::spin_some(eval_node); + } + } + + // Latest metric value + bool metric_updated_ = false; + double metric_value_; + // Node + rclcpp::Node::SharedPtr dummy_node; + EvalNode::SharedPtr eval_node; + + // Pub/Sub + rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Subscription::SharedPtr metric_sub_; + rclcpp::Subscription::SharedPtr marker_sub_; + bool has_received_marker_{false}; + + double time_delay_ = 5.0; + double time_step_ = 0.5; + double time_horizon_ = 10.0; + double velocity_ = 2.0; + + unique_identifier_msgs::msg::UUID uuid_; +}; + +// ========================================================================================== +// lateral deviation +TEST_F(EvalTest, testLateralDeviation_deviation0) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_deviation1) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_oscillation) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } else { + objects = makeDeviatedStraightPredictedObjects(time, deviation * sign); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testLateralDeviation_distortion) +{ + waitForDummyNode(); + setTargetMetric("lateral_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, deviation); + } else if (time == time_delay_ + time_step_) { + objects = makeDeviatedStraightPredictedObjects(time, -deviation); + } else { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), deviation, epsilon); +} +// ========================================================================================== + +// ========================================================================================== +// yaw deviation +TEST_F(EvalTest, testYawDeviation_deviation0) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 0.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_deviation1) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_oscillation) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } else { + objects = makeDeviatedStraightPredictedObjects(time, deviation * sign); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0.0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_distortion) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = makeDeviatedStraightPredictedObjects(time, deviation); + } else if (time == time_delay_ + time_step_) { + objects = makeDeviatedStraightPredictedObjects(time, -deviation); + } else { + objects = makeDeviatedStraightPredictedObjects(time, 0); + } + publishObjects(objects); + } + + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), 0, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_oscillation_rotate) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + const double yaw = M_PI / 4; + double sign = 1.0; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), yaw); + } else { + objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time, deviation * sign), 2 * M_PI * std::rand()); + sign *= -1.0; + } + publishObjects(objects); + } + + const auto last_objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); +} + +TEST_F(EvalTest, testYawDeviation_distortion_rotate) +{ + waitForDummyNode(); + setTargetMetric("yaw_deviation"); + + const double deviation = 1.0; + const double yaw = M_PI / 4; + for (double time = 0; time < time_delay_ * 2; time += time_step_) { + PredictedObjects objects; + if (time == time_delay_) { + objects = rotateObjects(makeDeviatedStraightPredictedObjects(time, deviation), yaw); + } else if (time == time_delay_ + time_step_) { + objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time, -deviation), 2 * M_PI * std::rand()); + } else { + objects = + rotateObjects(makeDeviatedStraightPredictedObjects(time, 0), 2 * M_PI * std::rand()); + } + publishObjects(objects); + } + + const auto last_objects = rotateObjects( + makeDeviatedStraightPredictedObjects(time_delay_ * 2, deviation), 2 * M_PI * std::rand()); + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), yaw, epsilon); +} + +// ========================================================================================== +// predicted path deviation{ +TEST_F(EvalTest, testPredictedPathDeviation_deviation0) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 0.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviation_deviation1) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 1.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} + +TEST_F(EvalTest, testPredictedPathDeviation_deviation2) +{ + waitForDummyNode(); + + setTargetMetric("predicted_path_deviation_5.00"); + + const auto init_objects = makeStraightPredictedObjects(0); + publishObjects(init_objects); + + const double deviation = 2.0; + for (double time = time_step_; time < time_delay_; time += time_step_) { + const auto objects = makeDeviatedStraightPredictedObjects(time, deviation); + publishObjects(objects); + } + const auto last_objects = makeDeviatedStraightPredictedObjects(time_delay_, deviation); + + const double num_points = time_delay_ / time_step_ + 1; + const double mean_deviation = deviation * (num_points - 1) / num_points; + EXPECT_NEAR(publishObjectsAndGetMetric(last_objects), mean_deviation, epsilon); +} +// ========================================================================================== diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index dbb74335f79d2..16396ef2961d8 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -247,4 +247,9 @@ + + + + + diff --git a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml index ec7545956e774..0885f48a6827d 100644 --- a/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/mission_planning/mission_planning.launch.xml @@ -1,19 +1,10 @@ - - - - - - - - - - - - - - + + + + + diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index e02ba883d5115..710fb20631a45 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -9,6 +9,7 @@ + @@ -25,6 +26,7 @@ + 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 bae084f80b51e..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,5 +1,6 @@ + @@ -11,6 +12,7 @@ + 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 90ebd23215be5..d2aa009d9c699 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 @@ -5,6 +5,7 @@ + @@ -203,6 +204,7 @@ + @@ -245,6 +247,7 @@ + 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 e673d28804480..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,5 +1,6 @@ + @@ -53,6 +54,7 @@ + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 4da6720b0af47..3801c448eaed4 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -85,6 +85,11 @@ + + + + + diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 38b5a932ae91d..04c762ae950a4 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -55,7 +55,7 @@ class MapUpdateModule friend class NDTScanMatcher; // Update the specified NDT - void update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt); + bool update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt); void update_map(const geometry_msgs::msg::Point & position); [[nodiscard]] bool should_update_map(const geometry_msgs::msg::Point & position); void publish_partial_pcd_map(); diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 17bafcfedfe80..950457f0d49b1 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -70,13 +70,16 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) // lock and rebuild ndt_ptr_ if (need_rebuild_) { ndt_ptr_mutex_->lock(); + auto param = ndt_ptr_->getParams(); + auto input_source = ndt_ptr_->getInputSource(); ndt_ptr_.reset(new NdtType); ndt_ptr_->setParams(param); update_ndt(position, *ndt_ptr_); + ndt_ptr_->setInputSource(input_source); ndt_ptr_mutex_->unlock(); need_rebuild_ = false; } else { @@ -85,13 +88,20 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) // the main ndt_ptr_) overlap, the latency of updating/alignment reduces partly. // If the updating is done the main ndt_ptr_, either the update or the NDT // align will be blocked by the other. - update_ndt(position, *secondary_ndt_ptr_); + const bool updated = update_ndt(position, *secondary_ndt_ptr_); + if (!updated) { + last_update_position_ = position; + return; + } ndt_ptr_mutex_->lock(); + auto dummy_ptr = ndt_ptr_; auto input_source = ndt_ptr_->getInputSource(); ndt_ptr_ = secondary_ndt_ptr_; ndt_ptr_->setInputSource(input_source); ndt_ptr_mutex_->unlock(); + + dummy_ptr.reset(); } secondary_ndt_ptr_.reset(new NdtType); @@ -104,7 +114,7 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) publish_partial_pcd_map(); } -void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt) +bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, NdtType & ndt) { auto request = std::make_shared(); @@ -126,7 +136,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt while (status != std::future_status::ready) { RCLCPP_INFO(logger_, "waiting response"); if (!rclcpp::ok()) { - return; + return false; // No update } status = result.wait_for(std::chrono::seconds(1)); } @@ -138,7 +148,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); if (maps_to_add.empty() && map_ids_to_remove.empty()) { RCLCPP_INFO(logger_, "Skip map update"); - return; + return false; // No update } const auto exe_start_time = std::chrono::system_clock::now(); @@ -168,6 +178,7 @@ void MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt std::chrono::duration_cast(exe_end_time - exe_start_time).count(); const auto exe_time = static_cast(duration_micro_sec) / 1000.0; RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + return true; // Updated } void MapUpdateModule::publish_partial_pcd_map() diff --git a/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json index 10454b48d3d80..b389e496fc477 100644 --- a/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json +++ b/perception/compare_map_segmentation/schema/distance_based_compare_map_filter.schema.json @@ -15,9 +15,36 @@ "type": "boolean", "default": "true", "description": "map loading mode selection, true for dynamic map loading, false for static map loading, recommended for no-split map pointcloud" + }, + "timer_interval_ms": { + "type": "number", + "default": "100", + "description": "Timer interval to load map points [ms]" + }, + "map_update_distance_threshold": { + "type": "number", + "default": "10.0", + "description": "Threshold distance to update map points with input points [m]" + }, + "map_loader_radius": { + "type": "number", + "default": "150.0", + "description": "Radius to load map points [m]" + }, + "publish_debug_pcd": { + "type": "boolean", + "default": "false", + "description": "Publish a downsampled map pointcloud for debugging" } }, - "required": ["distance_threshold", "use_dynamic_map_loading"], + "required": [ + "distance_threshold", + "use_dynamic_map_loading", + "timer_interval_ms", + "map_update_distance_threshold", + "map_loader_radius", + "publish_debug_pcd" + ], "additionalProperties": false } }, diff --git a/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json index 3c301d5ad9fbb..1b2e8ba5d51f8 100644 --- a/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json +++ b/perception/compare_map_segmentation/schema/voxel_based_approximate_compare_map_filter.schema.json @@ -20,9 +20,37 @@ "type": "number", "default": "0.5", "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + }, + "timer_interval_ms": { + "type": "number", + "default": "100", + "description": "Timer interval to load map points [ms]" + }, + "map_update_distance_threshold": { + "type": "number", + "default": "10.0", + "description": "Threshold distance to update map points with input points [m]" + }, + "map_loader_radius": { + "type": "number", + "default": "150.0", + "description": "Radius to load map points [m]" + }, + "publish_debug_pcd": { + "type": "boolean", + "default": "false", + "description": "Publish a downsampled map pointcloud for debugging" } }, - "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "required": [ + "distance_threshold", + "use_dynamic_map_loading", + "downsize_ratio_z_axis", + "timer_interval_ms", + "map_update_distance_threshold", + "map_loader_radius", + "publish_debug_pcd" + ], "additionalProperties": false } }, diff --git a/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json index 2e541662a1743..8cf95b8a3098e 100644 --- a/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json +++ b/perception/compare_map_segmentation/schema/voxel_based_compare_map_filter.schema.json @@ -21,6 +21,11 @@ "default": "0.5", "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" }, + "timer_interval_ms": { + "type": "number", + "default": "100", + "description": "Timer interval to load map points [ms]" + }, "map_update_distance_threshold": { "type": "number", "default": "10.0", @@ -31,19 +36,20 @@ "default": "150.0", "description": "Radius to load map points [m]" }, - "timer_interval_ms": { - "type": "number", - "default": "100", - "description": "Timer interval to load map points [ms]" + "publish_debug_pcd": { + "type": "boolean", + "default": "false", + "description": "Publish a downsampled map pointcloud for debugging" } }, "required": [ "distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis", + "timer_interval_ms", "map_update_distance_threshold", "map_loader_radius", - "timer_interval_ms" + "publish_debug_pcd" ], "additionalProperties": false } diff --git a/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json index 4663dbe806223..d743fb45cfd60 100644 --- a/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json +++ b/perception/compare_map_segmentation/schema/voxel_distance_based_compare_map_filter.schema.json @@ -20,9 +20,37 @@ "type": "number", "default": "0.5", "description": "Positive ratio to reduce voxel_leaf_size and neighbor point distance threshold in z axis" + }, + "timer_interval_ms": { + "type": "number", + "default": "100", + "description": "Timer interval to load map points [ms]" + }, + "map_update_distance_threshold": { + "type": "number", + "default": "10.0", + "description": "Threshold distance to update map points with input points [m]" + }, + "map_loader_radius": { + "type": "number", + "default": "150.0", + "description": "Radius to load map points [m]" + }, + "publish_debug_pcd": { + "type": "boolean", + "default": "false", + "description": "Publish a downsampled map pointcloud for debugging" } }, - "required": ["distance_threshold", "use_dynamic_map_loading", "downsize_ratio_z_axis"], + "required": [ + "distance_threshold", + "use_dynamic_map_loading", + "downsize_ratio_z_axis", + "timer_interval_ms", + "map_update_distance_threshold", + "map_loader_radius", + "publish_debug_pcd" + ], "additionalProperties": false } }, diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml index 31bb633294748..3e494a78cd5ad 100644 --- a/perception/detected_object_validation/package.xml +++ b/perception/detected_object_validation/package.xml @@ -7,6 +7,7 @@ Yukihiro Saito Shunsuke Miura badai nguyen + Shintaro Tomie Apache License 2.0 ament_cmake_auto diff --git a/perception/detection_by_tracker/package.xml b/perception/detection_by_tracker/package.xml index 21db6feeb4e8d..dae58f2d7da78 100644 --- a/perception/detection_by_tracker/package.xml +++ b/perception/detection_by_tracker/package.xml @@ -6,6 +6,7 @@ The ROS 2 detection_by_tracker package Yukihiro Saito Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto diff --git a/perception/elevation_map_loader/package.xml b/perception/elevation_map_loader/package.xml index 6e271b8528c47..f475ea4b3ceb1 100644 --- a/perception/elevation_map_loader/package.xml +++ b/perception/elevation_map_loader/package.xml @@ -6,6 +6,7 @@ The map_loader package Kosuke Takeuchi Taichi Higashide + Shintaro Tomie Apache License 2.0 ament_cmake_auto diff --git a/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp b/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp index d7541df046532..229a11645a215 100644 --- a/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp +++ b/perception/lidar_apollo_segmentation_tvm/data/models/baidu_cnn/inference_engine_tvm_config.hpp @@ -17,6 +17,8 @@ #ifndef PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT #define PERCEPTION__LIDAR_APOLLO_SEGMENTATION_TVM__DATA__MODELS__BAIDU_CNN__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +// cspell: ignore bcnn + namespace model_zoo { namespace perception @@ -38,6 +40,7 @@ static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ "./deploy_graph.json", // network_graph_path "./deploy_param.params", // network_params_path + // cspell: ignore DLCPU kDLCPU, // tvm_device_type 0, // tvm_device_id diff --git a/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp index 8201dd25c1039..5d832027c7591 100644 --- a/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp +++ b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/inference_engine_tvm_config.hpp @@ -38,6 +38,7 @@ static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ "./deploy_graph.json", // network_graph_path "./deploy_param.params", // network_params_path + // cspell: ignore DLCPU kDLCPU, // tvm_device_type 0, // tvm_device_id diff --git a/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp index 2f6943e90bc83..0cae27b49572c 100644 --- a/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp +++ b/perception/lidar_centerpoint_tvm/data/models/centerpoint_backbone/preprocessing_inference_engine_tvm_config.hpp @@ -40,6 +40,7 @@ static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ "./", // network_graph_path "./", // network_params_path + // cspell: ignore DLCPU kDLCPU, // tvm_device_type 0, // tvm_device_id diff --git a/perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp b/perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp index 521036b49a533..3e423dbe99877 100644 --- a/perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp +++ b/perception/lidar_centerpoint_tvm/data/models/centerpoint_encoder/inference_engine_tvm_config.hpp @@ -38,6 +38,7 @@ static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ "./deploy_graph.json", // network_graph_path "./deploy_param.params", // network_params_path + // cspell: ignore DLCPU kDLCPU, // tvm_device_type 0, // tvm_device_id diff --git a/perception/map_based_prediction/images/inside_road.svg b/perception/map_based_prediction/images/inside_road.svg index 2336813635269..d5383a419aa90 100644 --- a/perception/map_based_prediction/images/inside_road.svg +++ b/perception/map_based_prediction/images/inside_road.svg @@ -211,7 +211,7 @@ >
- output predicted paths toward　both exit points + output predicted paths toward both exit points
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..e7bf8030cddf8 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 @@ -79,7 +79,7 @@ class Tracker const rclcpp::Time & time) const; /* - *　Pure virtual function + * Pure virtual function */ protected: diff --git a/perception/multi_object_tracker/package.xml b/perception/multi_object_tracker/package.xml index 763bd12fab79e..699b28222f63f 100644 --- a/perception/multi_object_tracker/package.xml +++ b/perception/multi_object_tracker/package.xml @@ -6,6 +6,7 @@ The ROS 2 multi_object_tracker package Yukihiro Saito Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto 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 57c2e8f899951..7562684b84220 100644 --- a/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp +++ b/perception/multi_object_tracker/src/tracker/model/pedestrian_tracker.cpp @@ -158,6 +158,7 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) bool PedestrianTracker::predict(const double dt, KalmanFilter & ekf) const { + // cspell: ignore CTRV /* Motion model: Constant turn-rate motion model (CTRV) * * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt diff --git a/perception/object_merger/package.xml b/perception/object_merger/package.xml index 2c10f6c7c1160..5859e073a74ce 100644 --- a/perception/object_merger/package.xml +++ b/perception/object_merger/package.xml @@ -6,6 +6,7 @@ The object_merger package Yukihiro Saito Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp index 8f09764688055..19311559a3e7d 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/fusion/synchronized_grid_map_fusion_node.hpp @@ -43,6 +43,8 @@ #include #include +// cspell: ignore LOBF + namespace synchronized_grid_map_fusion { diff --git a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp index f9f100f285d38..245484e442609 100644 --- a/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/probabilistic_occupancy_grid_map/updater/occupancy_grid_map_log_odds_bayes_filter_updater.hpp @@ -23,6 +23,7 @@ #include // LOBF means: Log Odds Bayes Filter +// cspell: ignore LOBF namespace costmap_2d { diff --git a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index a88e65e712ac1..28fe4adafb482 100644 --- a/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -17,6 +17,8 @@ #include "probabilistic_occupancy_grid_map/cost_value.hpp" #include "probabilistic_occupancy_grid_map/utils/utils.hpp" +// cspell: ignore LOBF + namespace synchronized_grid_map_fusion { using costmap_2d::OccupancyGridMapFixedBlindSpot; diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 0b074f2a7ed7c..96afb0f2e852c 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -175,7 +175,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( Pose gridmap_origin{}; Pose scan_origin{}; try { - robot_pose = utils::getPose(input_raw_msg->header, *tf2_, map_frame_); + robot_pose = utils::getPose(input_raw_msg->header.stamp, *tf2_, base_link_frame_, map_frame_); gridmap_origin = utils::getPose(input_raw_msg->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_); scan_origin = diff --git a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp index f3e306f262bf0..607694d6da571 100644 --- a/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/updater/occupancy_grid_map_log_odds_bayes_filter_updater.cpp @@ -18,6 +18,8 @@ #include +// cspell: ignore LOBF + namespace costmap_2d { diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index dd179e99829a3..bf4a46cda7ae6 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -17,9 +17,17 @@ rclcpp_components_register_node(radar_crossing_objects_noise_filter_node_compone # Tests if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(radar_crossing_objects_noise_filter ${test_files}) + + target_link_libraries(radar_crossing_objects_noise_filter + radar_crossing_objects_noise_filter_node_component + ) endif() # Package diff --git a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp b/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp index 3b4674072a742..7ef369840c791 100644 --- a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp +++ b/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp @@ -58,6 +58,7 @@ class RadarCrossingObjectsNoiseFilterNode : public rclcpp::Node // Parameter NodeParam node_param_{}; +public: // Core bool isNoise(const DetectedObject & object); }; diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp new file mode 100644 index 0000000000000..43f030ce8493a --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp @@ -0,0 +1,26 @@ +// Copyright 2024 TierIV +// +// 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 + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp new file mode 100644 index 0000000000000..16d3cad8c3314 --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -0,0 +1,175 @@ +// 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 "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" + +#include + +#include + +std::shared_ptr get_node( + double angle_threshold, double velocity_threshold) +{ + rclcpp::NodeOptions node_options; + + node_options.parameter_overrides( + {{"angle_threshold", angle_threshold}, {"velocity_threshold", velocity_threshold}}); + auto node = + std::make_shared( + node_options); + return node; +} + +autoware_auto_perception_msgs::msg::DetectedObject get_object( + geometry_msgs::msg::Vector3 velocity, geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation) +{ + autoware_auto_perception_msgs::msg::DetectedObject object; + object.kinematics.twist_with_covariance.twist.linear = velocity; + object.kinematics.pose_with_covariance.pose.position = position; + object.kinematics.pose_with_covariance.pose.orientation = orientation; + return object; +} + +TEST(RadarCrossingObjectsFilter, IsNoise) +{ + rclcpp::init(0, nullptr); + { + auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + } + + { + auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + } + + { + auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + } + + { + auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + } +} diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp index 4e113ac47f57f..12cdd4bfe3d73 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/tracker_base.hpp @@ -79,7 +79,7 @@ class Tracker const rclcpp::Time & time) const; /* - *　Pure virtual function + * Pure virtual function */ protected: diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 6939c54b5be75..a371d9054966a 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -7,6 +7,7 @@ Yoshi Ri Yukihiro Saito Satoshi Tanaka + Taekjin Lee Apache License 2.0 ament_cmake_auto diff --git a/perception/tensorrt_classifier/package.xml b/perception/tensorrt_classifier/package.xml index 27513652ae829..439318c147e76 100644 --- a/perception/tensorrt_classifier/package.xml +++ b/perception/tensorrt_classifier/package.xml @@ -6,7 +6,8 @@ Dan Umeda Mingyu Li - >Mingyu Li + Kotaro Uetake + Shunsuke Miura Apache License 2.0 diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 21860854ccd5f..a5498a845e62e 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -114,4 +114,5 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml b/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml new file mode 100644 index 0000000000000..bc67173442094 --- /dev/null +++ b/perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx" + label_path: "$(var data_path)/tensorrt_yolox/label.txt" + score_threshold: 0.35 + nms_threshold: 0.7 + precision: "int8" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]. + calibration_algorithm: "Entropy" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]. + dla_core_id: -1 # If positive ID value is specified, the node assign inference task to the DLA core. + quantize_first_layer: false # If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8. + quantize_last_layer: false # If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8. + profile_per_layer: false # If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose. + clip_value: 6.0 # If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration. + preprocess_on_gpu: true # If true, pre-processing is performed on GPU. + calibration_image_list_path: "" # Path to a file which contains path to images. Those images will be used for int8 quantization. diff --git a/perception/tensorrt_yolox/config/yolox_tiny.param.yaml b/perception/tensorrt_yolox/config/yolox_tiny.param.yaml new file mode 100644 index 0000000000000..e45742a7afb95 --- /dev/null +++ b/perception/tensorrt_yolox/config/yolox_tiny.param.yaml @@ -0,0 +1,15 @@ +/**: + ros__parameters: + model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx" + label_path: "$(var data_path)/tensorrt_yolox/label.txt" + score_threshold: 0.35 + nms_threshold: 0.7 + precision: "fp16" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]. + calibration_algorithm: "MinMax" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]. + dla_core_id: -1 # If positive ID value is specified, the node assign inference task to the DLA core. + quantize_first_layer: false # If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8. + quantize_last_layer: false # If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8. + profile_per_layer: false # If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose. + clip_value: 0.0 # If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration. + preprocess_on_gpu: true # If true, pre-processing is performed on GPU. + calibration_image_list_path: "" # Path to a file which contains path to images. Those images will be used for int8 quantization. diff --git a/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml b/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml index 5959f73757de1..9de37c76e034b 100644 --- a/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml +++ b/perception/tensorrt_yolox/launch/multiple_yolox.launch.xml @@ -27,43 +27,43 @@ - - + + - - + + - - + + - - + + - - + + - - + + - - + + - - + + diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 3f8d7897ab5d3..dd15eda2913ce 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -5,30 +5,7 @@ - - - - - - - - - - - - + @@ -40,19 +17,7 @@ - - - - - - - - - - - - - + diff --git a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml index 2f08031ea159f..9e5d1c371b13b 100644 --- a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml @@ -4,30 +4,7 @@ - - - - - - - - - - - - + @@ -39,19 +16,7 @@ - - - - - - - - - - - - - + diff --git a/perception/tensorrt_yolox/package.xml b/perception/tensorrt_yolox/package.xml index e262dd90c9681..2afbfcb9fe3ed 100644 --- a/perception/tensorrt_yolox/package.xml +++ b/perception/tensorrt_yolox/package.xml @@ -5,7 +5,6 @@ tensorrt library implementation for yolox Daisuke Nishimatsu - Daisuke Nishimatsu Dan Umeda Manato Hirabayashi diff --git a/perception/tensorrt_yolox/schema/yolox_s_plus_opt.schema.json b/perception/tensorrt_yolox/schema/yolox_s_plus_opt.schema.json new file mode 100644 index 0000000000000..ce1ad6c2d0caf --- /dev/null +++ b/perception/tensorrt_yolox/schema/yolox_s_plus_opt.schema.json @@ -0,0 +1,107 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for tensorrt_yolox_s_plus_opt Nodes", + "type": "object", + "definitions": { + "yolox_s_plus_opt": { + "type": "object", + "properties": { + "model_path": { + "type": "string", + "default": "$(var data_path)/tensorrt_yolox/$(var model_name).onnx", + "description": "Path to onnx model." + }, + "label_path": { + "type": "string", + "default": "$(var data_path)/tensorrt_yolox/label.txt", + "description": "Path to label file." + }, + "score_threshold": { + "type": "number", + "default": 0.35, + "minimum": 0.0, + "maximum": 1.0, + "description": "A threshold value of existence probability score, all of objects with score less than this threshold are ignored." + }, + "nms_threshold": { + "type": "number", + "default": 0.7, + "minimum": 0.0, + "maximum": 1.0, + "description": "A threshold value of NMS." + }, + "precision": { + "type": "string", + "default": "int8", + "description": "Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]." + }, + "calibration_algorithm": { + "type": "string", + "default": "Entropy", + "description": "Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]." + }, + "dla_core_id": { + "type": "number", + "default": -1, + "description": "If positive ID value is specified, the node assign inference task to the DLA core." + }, + "quantize_first_layer": { + "type": "boolean", + "default": false, + "description": "If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8." + }, + "quantize_last_layer": { + "type": "boolean", + "default": false, + "description": "If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8." + }, + "profile_per_layer": { + "type": "boolean", + "default": false, + "description": "If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose." + }, + "clip_value": { + "type": "number", + "default": 6.0, + "description": "If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration." + }, + "preprocess_on_gpu": { + "type": "boolean", + "default": true, + "description": "If true, pre-processing is performed on GPU." + }, + "calibration_image_list_path": { + "type": "string", + "default": "", + "description": "Path to a file which contains path to images. Those images will be used for int8 quantization." + } + }, + "required": [ + "model_path", + "label_path", + "score_threshold", + "nms_threshold", + "precision", + "calibration_algorithm", + "dla_core_id", + "quantize_first_layer", + "quantize_last_layer", + "profile_per_layer", + "clip_value", + "preprocess_on_gpu" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/yolox_s_plus_opt" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/tensorrt_yolox/schema/yolox_tiny.schema.json b/perception/tensorrt_yolox/schema/yolox_tiny.schema.json new file mode 100644 index 0000000000000..f47b28e47a3f8 --- /dev/null +++ b/perception/tensorrt_yolox/schema/yolox_tiny.schema.json @@ -0,0 +1,107 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for tensorrt_yolox_tiny Nodes", + "type": "object", + "definitions": { + "yolox_tiny": { + "type": "object", + "properties": { + "model_path": { + "type": "string", + "default": "$(var data_path)/tensorrt_yolox/$(var model_name).onnx", + "description": "Path to onnx model." + }, + "label_path": { + "type": "string", + "default": "$(var data_path)/tensorrt_yolox/label.txt", + "description": "Path to label file." + }, + "score_threshold": { + "type": "number", + "default": 0.35, + "minimum": 0.0, + "maximum": 1.0, + "description": "A threshold value of existence probability score, all of objects with score less than this threshold are ignored." + }, + "nms_threshold": { + "type": "number", + "default": 0.7, + "minimum": 0.0, + "maximum": 1.0, + "description": "A threshold value of NMS." + }, + "precision": { + "type": "string", + "default": "fp16", + "description": "Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]." + }, + "calibration_algorithm": { + "type": "string", + "default": "MinMax", + "description": "Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]." + }, + "dla_core_id": { + "type": "number", + "default": -1, + "description": "If positive ID value is specified, the node assign inference task to the DLA core." + }, + "quantize_first_layer": { + "type": "boolean", + "default": false, + "description": "If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8." + }, + "quantize_last_layer": { + "type": "boolean", + "default": false, + "description": "If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8." + }, + "profile_per_layer": { + "type": "boolean", + "default": false, + "description": "If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose." + }, + "clip_value": { + "type": "number", + "default": 0.0, + "description": "If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration." + }, + "preprocess_on_gpu": { + "type": "boolean", + "default": true, + "description": "If true, pre-processing is performed on GPU." + }, + "calibration_image_list_path": { + "type": "string", + "default": "", + "description": "Path to a file which contains path to images. Those images will be used for int8 quantization." + } + }, + "required": [ + "model_path", + "label_path", + "score_threshold", + "nms_threshold", + "precision", + "calibration_algorithm", + "dla_core_id", + "quantize_first_layer", + "quantize_last_layer", + "profile_per_layer", + "clip_value", + "preprocess_on_gpu" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/yolox_tiny" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml index 58184c090a1e1..027a7bf26d2c4 100644 --- a/perception/tracking_object_merger/package.xml +++ b/perception/tracking_object_merger/package.xml @@ -6,6 +6,7 @@ merge tracking object Yukihiro Saito Yoshi Ri + Taekjin Lee Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_fine_detector/package.xml b/perception/traffic_light_fine_detector/package.xml index 8ff9f2d133b52..cf2495228b0c5 100644 --- a/perception/traffic_light_fine_detector/package.xml +++ b/perception/traffic_light_fine_detector/package.xml @@ -6,6 +6,7 @@ The traffic_light_fine_detector package Tao Zhong Shunsuke Miura + Shintaro Tomie Apache License 2.0 ament_cmake_auto diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index c2d53869fa3d5..4631935fe42fa 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -5,6 +5,7 @@ 0.1.0 The traffic_light_map_based_detector package Yukihiro Saito + Shunsuke Miura Apache License 2.0 ament_cmake_auto diff --git a/planning/.pages b/planning/.pages index 4be0dbfeeaae0..1b5608f7ded57 100644 --- a/planning/.pages +++ b/planning/.pages @@ -77,8 +77,8 @@ nav: - 'Additional Tools': - 'RTC Replayer': planning/rtc_replayer - 'Planning Debug Tools': - - ['About Planning Debug Tools'](https://github.com/autowarefoundation/autoware_tools/tree/main/planning_debug_tools) - - ['Stop Reason Visualizer'](https://github.com/autowarefoundation/autoware_tools/tree/main/planning_debug_tools/doc-stop-reason-visualizer) + - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools + - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - 'Planning Test Utils': planning/planning_test_utils - 'Planning Topic Converter': planning/planning_topic_converter - 'Planning Validator': planning/planning_validator diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp index 7a82ba60a1eaa..c663c31537fd1 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp @@ -54,6 +54,10 @@ class AvoidanceByLaneChange : public NormalLaneChange const AvoidancePlanningData & data, const PredictedObject & object) const; void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const; + + double calcMinAvoidanceLength(const ObjectData & nearest_object) const; + double calcMinimumLaneChangeLength() const; + double calcLateralOffset() const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp index efdb302a58ac2..3a7c22c84b3e8 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -16,13 +16,9 @@ #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/marker_utils/utils.hpp" -#include #include #include -#include -#include namespace behavior_path_planner { @@ -45,7 +41,8 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { - return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck(); + return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && + module_type_->isValidPath(); } void AvoidanceByLaneChangeInterface::processOnEntry() { 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 9f3966e055d9b..fc755bc4115a9 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,16 +20,21 @@ #include "behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_planner_common/utils/utils.hpp" +#include +#include #include #include #include #include +#include #include namespace behavior_path_planner { +using behavior_path_planner::utils::lane_change::debug::createExecutionArea; + AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, std::shared_ptr avoidance_parameters) @@ -68,44 +73,15 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const return false; } - const auto current_lanes = getCurrentLanes(); - if (current_lanes.empty()) { - RCLCPP_DEBUG(logger_, "no empty lanes"); - return false; - } - const auto & nearest_object = data.target_objects.front(); + const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); + const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); - // get minimum lane change distance - const auto shift_intervals = - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); - const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, shift_intervals, - lane_change_parameters_->backward_length_buffer_for_end_of_lane); - - // get minimum avoid distance - - const auto ego_width = getCommonParam().vehicle_width; - 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; - - avoidance_helper_->setData(planner_data_); - const auto shift_length = avoidance_helper_->getShiftLength( - nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); - - const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length); - - RCLCPP_DEBUG( - logger_, - "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance " - "%.3f", - nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance); + lane_change_debug_.execution_area = createExecutionArea( + getCommonParam().vehicle_info, getEgoPose(), + std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); - return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance); + return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length); } bool AvoidanceByLaneChange::specialExpiredCheck() const @@ -274,4 +250,48 @@ ObjectData AvoidanceByLaneChange::createObjectData( return object_data; } + +double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const +{ + const auto ego_width = getCommonParam().vehicle_width; + 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; + + avoidance_helper_->setData(planner_data_); + const auto shift_length = avoidance_helper_->getShiftLength( + nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); + + return avoidance_helper_->getMinAvoidanceDistance(shift_length); +} + +double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const +{ + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger_, "no empty lanes"); + return std::numeric_limits::infinity(); + } + + // get minimum lane change distance + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + return utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, + lane_change_parameters_->backward_length_buffer_for_end_of_lane); +} + +double AvoidanceByLaneChange::calcLateralOffset() const +{ + auto additional_lat_offset{0.0}; + for (const auto & [type, p] : avoidance_parameters_->object_parameters) { + const auto offset = + 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; + additional_lat_offset = std::max(additional_lat_offset, offset); + } + return additional_lat_offset; +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_avoidance_module/README.md b/planning/behavior_path_avoidance_module/README.md index 122ad7adcce9e..dc6ab0ec4b6e1 100644 --- a/planning/behavior_path_avoidance_module/README.md +++ b/planning/behavior_path_avoidance_module/README.md @@ -6,7 +6,7 @@ This is a rule-based path planning module designed for obstacle avoidance. This module is designed for rule-based avoidance that is easy for developers to design its behavior. It generates avoidance path parameterized by intuitive parameters such as lateral jerk and avoidance distance margin. This makes it possible to pre-define avoidance behavior. -In addition, the approval interface of behavior_path_planner allows external users / modules (e.g. remote operation) to intervene the decision of the vehicle behavior.　 This function is expected to be used, for example, for remote intervention in emergency situations or gathering information on operator decisions during development. +In addition, the approval interface of behavior_path_planner allows external users / modules (e.g. remote operation) to intervene the decision of the vehicle behavior. This function is expected to be used, for example, for remote intervention in emergency situations or gathering information on operator decisions during development. ### Limitations @@ -934,7 +934,7 @@ namespace: `avoidance.constraints.longitudinal.` ## Future extensions / Unimplemented parts - **Planning on the intersection** - - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop.　 This is especially important at intersections. + - If it is known that the ego vehicle is going to stop in the middle of avoidance execution (for example, at a red traffic light), sometimes the avoidance should not be executed until the vehicle is ready to move. This is because it is impossible to predict how the environment will change during the stop. This is especially important at intersections. ![fig1](./images/intersection_problem.drawio.svg) @@ -956,7 +956,7 @@ namespace: `avoidance.constraints.longitudinal.` - Essentially, avoidance targets are judged based on whether they are static objects or not. For example, a vehicle waiting at a traffic light should not be avoided because we know that it will start moving in the future. However this decision cannot be made in the current Autoware due to the lack of the perception functions. Therefore, the current avoidance module limits the avoidance target to vehicles parked on the shoulder of the road, and executes avoidance only for vehicles that are stopped away from the center of the lane. However, this logic cannot avoid a vehicle that has broken down and is stopped in the center of the lane, which should be recognized as a static object by the perception module. There is room for improvement in the performance of this decision. - **Resampling path** - - Now the rough resolution resampling is processed to the output path in order to reduce the computational cost for the later modules. This resolution is set to a uniformly large value 　(e.g. `5m`), but small resolution should be applied for complex paths. + - Now the rough resolution resampling is processed to the output path in order to reduce the computational cost for the later modules. This resolution is set to a uniformly large value (e.g. `5m`), but small resolution should be applied for complex paths. ## How to debug diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index ab4eda81f93c6..5a81d7d972a0e 100644 --- a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml +++ b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml @@ -4,9 +4,6 @@ avoidance: resample_interval_for_planning: 0.3 # [m] resample_interval_for_output: 4.0 # [m] - drivable_area_right_bound_offset: 0.0 # [m] - drivable_area_left_bound_offset: 0.0 # [m] - # avoidance module common setting enable_bound_clipping: false enable_yield_maneuver: true @@ -208,6 +205,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] 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 a536fa1b568cb..e4df3a8069b16 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 @@ -276,6 +276,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}; @@ -443,9 +446,6 @@ struct AvoidLine : public ShiftLine // Distance from ego to end point in Frenet double end_longitudinal = 0.0; - // for unique_id - UUID id{}; - // for the case the point is created by merge other points std::vector parent_ids{}; @@ -542,6 +542,8 @@ struct AvoidancePlanningData bool valid{false}; + bool ready{false}; + bool success{false}; bool comfortable{false}; 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 cc00bfbb978f5..4f15261f42246 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 @@ -270,6 +270,33 @@ class AvoidanceHelper }); } + 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; 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 fcaad191b1a33..ce75fa03e96ad 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 @@ -258,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) 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 a74c3a69d7bce..82e2c7af32f8d 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 @@ -404,6 +404,8 @@ class AvoidanceModule : public SceneModuleInterface bool safe_{true}; + std::optional ignore_signal_{std::nullopt}; + std::shared_ptr helper_; std::shared_ptr parameters_; 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 02d8d68cf7a56..2d61571392eef 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 @@ -176,7 +176,7 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); -TurnSignalInfo calcTurnSignalInfo( +std::pair calcTurnSignalInfo( const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, const AvoidancePlanningData & data, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp index 75529c35c2af5..ed3dc8d34a698 100644 --- a/planning/behavior_path_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_module/src/scene.cpp @@ -133,7 +133,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 @@ -513,6 +513,7 @@ 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()); } void AvoidanceModule::fillEgoStatus( @@ -918,12 +919,26 @@ BehaviorModuleOutput AvoidanceModule::plan() BehaviorModuleOutput output; + const auto is_ignore_signal = [this](const UUID & uuid) { + if (!ignore_signal_.has_value()) { + return false; + } + + return ignore_signal_.value() == uuid; + }; + + const auto update_ignore_signal = [this](const UUID & uuid, const bool is_ignore) { + ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; + }; + // turn signal info if (path_shifter_.getShiftLines().empty()) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; - const auto new_signal = utils::avoidance::calcTurnSignalInfo( + const auto [new_signal, is_ignore] = utils::avoidance::calcTurnSignalInfo( linear_shift_path, path_shifter_.getShiftLines().front(), helper_->getEgoShift(), avoid_data_, planner_data_); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); @@ -931,6 +946,7 @@ BehaviorModuleOutput AvoidanceModule::plan() spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); + update_ignore_signal(path_shifter_.getShiftLines().front().id, is_ignore); } // sparse resampling for computational cost diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 6a44350dd48c6..df775ea6b2c0a 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -250,16 +250,37 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } +bool isAvoidShift( + 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; +} + +bool isReturnShift( + 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; +} + +bool isLeftMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) +{ + return start_shift_length > threshold && end_shift_length > threshold; +} + +bool isRightMiddleShift( + const double start_shift_length, const double end_shift_length, const double threshold) +{ + return start_shift_length < threshold && end_shift_length < threshold; +} + bool existShiftSideLane( const double start_shift_length, const double end_shift_length, const bool no_left_lanes, - const bool no_right_lanes) + const bool no_right_lanes, const double threshold) { - constexpr double THRESHOLD = 0.1; const auto relative_shift_length = end_shift_length - start_shift_length; - const auto avoid_shift = - std::abs(start_shift_length) < THRESHOLD && std::abs(end_shift_length) > THRESHOLD; - if (avoid_shift) { + 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; @@ -271,9 +292,7 @@ bool existShiftSideLane( } } - const auto return_shift = - std::abs(start_shift_length) > THRESHOLD && std::abs(end_shift_length) < THRESHOLD; - if (return_shift) { + 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; @@ -285,8 +304,7 @@ bool existShiftSideLane( } } - const auto left_middle_shift = start_shift_length > THRESHOLD && end_shift_length > THRESHOLD; - if (left_middle_shift) { + 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; @@ -298,8 +316,7 @@ bool existShiftSideLane( } } - const auto right_middle_shift = start_shift_length < THRESHOLD && end_shift_length < THRESHOLD; - if (right_middle_shift) { + 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; @@ -314,6 +331,23 @@ bool existShiftSideLane( return true; } +bool isNearEndOfShift( + const double start_shift_length, const double end_shift_length, const Point & ego_pos, + const lanelet::ConstLanelets & original_lanes, const double threshold) +{ + 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()); + }); +} + bool straddleRoadBound( const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, const vehicle_info_util::VehicleInfo & vehicle_info) @@ -2323,7 +2357,7 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } -TurnSignalInfo calcTurnSignalInfo( +std::pair calcTurnSignalInfo( const ShiftedPath & path, const ShiftLine & shift_line, const double current_shift_length, const AvoidancePlanningData & data, const std::shared_ptr & planner_data) { @@ -2335,22 +2369,22 @@ TurnSignalInfo calcTurnSignalInfo( if (shift_line.start_idx + 1 > path.shift_length.size()) { RCLCPP_WARN(rclcpp::get_logger(__func__), "index inconsistency."); - return {}; + 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 {}; + 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 {}; + 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 {}; + return std::make_pair(TurnSignalInfo{}, true); } const auto start_shift_length = path.shift_length.at(shift_line.start_idx); @@ -2359,12 +2393,12 @@ TurnSignalInfo calcTurnSignalInfo( // 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 {}; + return std::make_pair(TurnSignalInfo{}, true); } // If the vehicle does not shift anymore, we turn off the blinker if (std::fabs(path.shift_length.at(shift_line.end_idx) - current_shift_length) < THRESHOLD) { - return {}; + return std::make_pair(TurnSignalInfo{}, true); } const auto get_command = [](const auto & shift_length) { @@ -2379,7 +2413,7 @@ TurnSignalInfo calcTurnSignalInfo( p.vehicle_info.max_longitudinal_offset_m; if (signal_prepare_distance < ego_front_to_shift_start) { - return {}; + return std::make_pair(TurnSignalInfo{}, false); } const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; @@ -2396,12 +2430,12 @@ TurnSignalInfo calcTurnSignalInfo( turn_signal_info.turn_signal.command = get_command(relative_shift_length); if (!p.turn_signal_on_swerving) { - return turn_signal_info; + return std::make_pair(turn_signal_info, false); } lanelet::ConstLanelet lanelet; if (!rh->getClosestLaneletWithinRoute(shift_line.end, &lanelet)) { - return {}; + return std::make_pair(TurnSignalInfo{}, true); } const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); @@ -2412,14 +2446,25 @@ TurnSignalInfo calcTurnSignalInfo( const auto has_right_lane = right_same_direction_lane.has_value() || !right_opposite_lanes.empty(); - if (!existShiftSideLane(start_shift_length, end_shift_length, !has_left_lane, !has_right_lane)) { - return {}; + if (!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, data.current_lanelets, p.vehicle_info)) { - return {}; + return std::make_pair(TurnSignalInfo{}, true); + } + + constexpr double STOPPED_THRESHOLD = 0.1; // [m/s] + if (ego_speed < STOPPED_THRESHOLD) { + if (isNearEndOfShift( + start_shift_length, end_shift_length, ego_pose.position, data.current_lanelets, + p.turn_signal_shift_length_threshold)) { + return std::make_pair(TurnSignalInfo{}, true); + } } - return turn_signal_info; + return std::make_pair(turn_signal_info, false); } } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index ef18d5b269be1..c8cef3d825490 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -214,7 +214,7 @@ The main thread will be the one called from the planner manager flow. | decide_path_distance | [m] | double | decide path if it approaches this distance relative to the parking position. after that, no path planning and goal search are performed | 10.0 | | maximum_deceleration | [m/s2] | double | maximum deceleration. it prevents sudden deceleration when a parking path cannot be found suddenly | 1.0 | | path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path which is set in `efficient_path_order`. In case `close_goal` use the closest goal to the original one. | efficient_path | -| efficient_path_order | [-] | string | efficient order of pull over planner along lanes　excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | +| efficient_path_order | [-] | string | efficient order of pull over planner along lanes excluding freespace pull over | ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] | ### **shift parking** @@ -366,18 +366,18 @@ This method integrates the footprints of egos and objects at a given time and ch In addition, the safety check has a time hysteresis, and if the path is judged "safe" for a certain period of time(`keep_unsafe_time`), it is finally treated as "safe". ```txt - 　　 ==== is_safe - 　　 ---- current_is_safe - 　　 is_safe - 　　 ^ - 　　 | - 　　 | time - 　　 1 +--+ +---+ +---========= +--+ - 　　 | | | | | | | | - 　　 | | | | | | | | - 　　 | | | | | | | | - 　　 | | | | | | | | - 　　 0 =========================-------==========--> t + ==== is_safe + ---- current_is_safe + is_safe + ^ + | + | time + 1 +--+ +---+ +---========= +--+ + | | | | | | | | + | | | | | | | | + | | | | | | | | + | | | | | | | | + 0 =========================-------==========--> t ``` ### Parameters for safety check diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index e487d9d2ae0b2..f228ecc378c28 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -36,7 +36,7 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; - // todo(kosuke55):　Functions for this specific use should not be in the interface, + // todo(kosuke55): Functions for this specific use should not be in the interface, // so it is better to consider interface design when we implement other goal searchers. GoalCandidate getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates) const override; 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 4fe77a4ee206c..ce0fc8cfbece1 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 @@ -2094,17 +2094,17 @@ std::pair GoalPlannerModule::isSafePath() const }); /* - *　　 ==== is_safe - *　　 ---- current_is_safe - *　　 is_safe - *　　 | - *　　 | time - *　　 1 +--+ +---+ +---========= +--+ - *　　 | | | | | | | | - *　　 | | | | | | | | - *　　 | | | | | | | | - *　　 | | | | | | | | - *　　 0 =========================-------==========-- t + * ==== is_safe + * ---- current_is_safe + * is_safe + * | + * | time + * 1 +--+ +---+ +---========= +--+ + * | | | | | | | | + * | | | | | | | | + * | | | | | | | | + * | | | | | | | | + * 0 =========================-------==========-- t */ if (current_is_safe) { if ( diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index 37d44cae32da3..8eb791ab4f5e9 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -163,7 +163,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light.　The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). ##### Collision check in prepare phase diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp index 8ac1ba3909a50..d8ba7cf19cfee 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -19,6 +19,8 @@ #include +#include + #include namespace behavior_path_planner::data::lane_change @@ -31,6 +33,7 @@ struct Debug CollisionCheckDebugMap collision_check_objects; CollisionCheckDebugMap collision_check_objects_after_approval; LaneChangeTargetObjects filtered_objects; + geometry_msgs::msg::Polygon execution_area; double collision_check_object_debug_lifetime{0.0}; void reset() diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index 427a26e9b4295..8863cabdba008 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -19,6 +19,7 @@ #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include @@ -40,6 +41,8 @@ MarkerArray showFilteredObjects( const ExtendedPredictedObjects & current_lane_objects, const ExtendedPredictedObjects & target_lane_objects, const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); +MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); MarkerArray createDebugMarkerArray(const Debug & debug_data); + } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ 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 e16afcdbf19ec..46b47de128bd9 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 @@ -225,4 +225,14 @@ lanelet::ConstLanelets generateExpandedLanelets( */ rclcpp::Logger getLogger(const std::string & type); } // namespace behavior_path_planner::utils::lane_change + +namespace behavior_path_planner::utils::lane_change::debug +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); + +geometry_msgs::msg::Polygon createExecutionArea( + const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose, + double additional_lon_offset, double additional_lat_offset); +} // namespace behavior_path_planner::utils::lane_change::debug + #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 6c4552de83ac1..2021630f1ae34 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -15,7 +15,6 @@ #include "behavior_path_lane_change_module/scene.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner_common/marker_utils/utils.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" @@ -33,7 +32,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 523b770734bc5..67594823eb112 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -162,6 +162,11 @@ MarkerArray createDebugMarkerArray(const Debug & debug_data) "ego_and_target_polygon_relation_after_approval")); } + if (!debug_data.execution_area.points.empty()) { + add(createPolygonMarkerArray( + debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); + } + return debug_marker; } } // namespace marker_utils::lane_change_markers 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 a331d418d5e46..48a5a8722b0e9 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1193,3 +1193,42 @@ rclcpp::Logger getLogger(const std::string & type) return rclcpp::get_logger("lane_change").get_child(type); } } // namespace behavior_path_planner::utils::lane_change + +namespace behavior_path_planner::utils::lane_change::debug +{ +geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Point32 p; + p.x = static_cast(pose.position.x); + p.y = static_cast(pose.position.y); + p.z = static_cast(pose.position.z); + return p; +}; + +geometry_msgs::msg::Polygon createExecutionArea( + const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose, + double additional_lon_offset, double additional_lat_offset) +{ + 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 + additional_lon_offset; + const double backward_lon_offset = -base_to_rear; + const double lat_offset = width / 2.0 + additional_lat_offset; + + const auto p1 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, lat_offset, 0.0); + const auto p2 = tier4_autoware_utils::calcOffsetPose(pose, forward_lon_offset, -lat_offset, 0.0); + const auto p3 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, -lat_offset, 0.0); + const auto p4 = tier4_autoware_utils::calcOffsetPose(pose, backward_lon_offset, lat_offset, 0.0); + geometry_msgs::msg::Polygon polygon; + + polygon.points.push_back(create_point32(p1)); + polygon.points.push_back(create_point32(p2)); + polygon.points.push_back(create_point32(p3)); + polygon.points.push_back(create_point32(p4)); + + return polygon; +} +} // namespace behavior_path_planner::utils::lane_change::debug 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 0c5dbc082c9b9..f3f6870085e02 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -25,11 +25,6 @@ input_path_interval: 2.0 output_path_interval: 2.0 - lane_following: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - drivable_area_types_to_skip: [road_border] - closest_lanelet: distance_threshold: 5.0 # [m] yaw_threshold: 0.79 # [rad] 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 3eb5eca5954fb..d2f58e7d7e017 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -505,7 +505,7 @@ void BehaviorPathPlannerNode::publish_reroute_availability() const { // In the current behavior path planner, we might encounter unexpected behavior when rerouting // while modules other than lane following are active. If non-lane-following module except - // always-executable module is approved and running, rerouting will not be　possible. + // always-executable module is approved and running, rerouting will not be possible. RerouteAvailability is_reroute_available; is_reroute_available.stamp = this->now(); if (planner_manager_->hasNonAlwaysExecutableApprovedModules()) { 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 26c0f247eb558..1489dd1beff07 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,8 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" +#include + #include #include #include @@ -72,7 +74,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) { @@ -115,7 +118,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_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 165a11ebb54b4..67f581123057e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -17,9 +17,11 @@ #include #include +#include #include #include +#include #include #include @@ -31,9 +33,13 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_autoware_utils::generateUUID; +using unique_identifier_msgs::msg::UUID; struct ShiftLine { + ShiftLine() : id(generateUUID()) {} + Pose start{}; // shift start point in absolute coordinate Pose end{}; // shift start point in absolute coordinate @@ -45,6 +51,9 @@ struct ShiftLine size_t start_idx{}; // associated start-point index for the reference path size_t end_idx{}; // associated end-point index for the reference path + + // for unique_id + UUID id{}; }; using ShiftLineArray = std::vector; diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index db5dc51150e55..2dd4d12c05487 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -49,6 +49,7 @@ freespace_planning_algorithms geometry_msgs interpolation + lane_departure_checker lanelet2_extension magic_enum motion_utils 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 a1a079e679e72..9400abd20b984 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 @@ -116,7 +116,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{}; } @@ -222,7 +222,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 @@ -242,7 +243,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; @@ -362,7 +363,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(); @@ -496,6 +498,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_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index 4db2390e5d91c..4b52aae91d032 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -332,6 +332,13 @@ PullOutPath --o PullOutPlannerBase | shift_collision_check_distance_from_end | [m] | double | collision check distance from end shift end pose | -10.0 | | geometric_collision_check_distance_from_end | [m] | double | collision check distance from end geometric end pose | 0.0 | | collision_check_margin_from_front_object | [m] | double | collision check margin from front object | 5.0 | +| object_types_to_check_for_path_generation.check_car | - | bool | flag to check car for path generation | true | +| object_types_to_check_for_path_generation.check_truck | - | bool | flag to check truck for path generation | true | +| object_types_to_check_for_path_generation.check_bus | - | bool | flag to check bus for path generation | true | +| object_types_to_check_for_path_generation.check_bicycle | - | bool | flag to check bicycle for path generation | true | +| object_types_to_check_for_path_generation.check_motorcycle | - | bool | flag to check motorcycle for path generation | true | +| object_types_to_check_for_path_generation.check_pedestrian | - | bool | flag to check pedestrian for path generation | true | +| object_types_to_check_for_path_generation.check_unknown | - | bool | flag to check unknown for path generation | true | | center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ### **Ego vehicle's velocity planning** @@ -433,16 +440,17 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral #### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | -| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | false | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | -| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true | +| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true | +| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 | +| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 | ### **geometric pull out** 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 2ff17bc508e89..fad29b84c9c76 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 @@ -8,11 +8,21 @@ collision_check_margins: [2.0, 1.0, 0.5, 0.1] collision_check_margin_from_front_object: 5.0 th_moving_object_velocity: 1.0 + object_types_to_check_for_path_generation: + check_car: true + check_truck: true + check_bus: true + check_trailer: true + check_bicycle: true + check_motorcycle: true + check_pedestrian: true + check_unknown: true th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: true + allow_check_shift_path_lane_departure_override: false shift_collision_check_distance_from_end: -10.0 minimum_shift_pull_out_distance: 0.0 deceleration_interval: 15.0 diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 563856c87db70..07c81d2edd050 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -67,11 +67,14 @@ struct StartPlannerParameters std::vector collision_check_margins{}; double collision_check_margin_from_front_object{0.0}; double th_moving_object_velocity{0.0}; + behavior_path_planner::utils::path_safety_checker::ObjectTypesToCheck + object_types_to_check_for_path_generation{}; double center_line_path_interval{0.0}; // shift pull out bool enable_shift_pull_out{false}; bool check_shift_path_lane_departure{false}; + bool allow_check_shift_path_lane_departure_override{false}; double shift_collision_check_distance_from_end{0.0}; double minimum_shift_pull_out_distance{0.0}; int lateral_acceleration_sampling_num{0}; 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/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 6bf85a4679dfd..2d50ae189dc13 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -50,11 +50,6 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); - void setDepartureCheckLanes(const lanelet::ConstLanelets & departure_check_lanes) - { - departure_check_lanes_ = departure_check_lanes; - } - std::shared_ptr lane_departure_checker_; private: @@ -63,8 +58,6 @@ class ShiftPullOut : public PullOutPlannerBase double calcPullOutLongitudinalDistance( const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; - - lanelet::ConstLanelets departure_check_lanes_; }; } // 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 a9eb81c3ba235..1368124929367 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 @@ -71,7 +71,7 @@ struct PullOutStatus std::shared_ptr prev_stop_path_after_approval{nullptr}; std::optional stop_pose{std::nullopt}; //! record the first time when the state changed from !isActivated() to isActivated() - std::optional first_approved_time{std::nullopt}; + std::optional first_engaged_time{std::nullopt}; PullOutStatus() {} }; @@ -178,6 +178,21 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; + uint16_t getSteeringFactorDirection( + const behavior_path_planner::BehaviorModuleOutput & output) const + { + switch (output.turn_signal_info.turn_signal.command) { + case TurnIndicatorsCommand::ENABLE_LEFT: + return SteeringFactor::LEFT; + + case TurnIndicatorsCommand::ENABLE_RIGHT: + return SteeringFactor::RIGHT; + + default: + return SteeringFactor::STRAIGHT; + } + }; + /** * @brief Check if there are no moving objects around within a certain radius. * @@ -275,7 +290,6 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - void updateDepartureCheckLanes(); lanelet::ConstLanelets createDepartureCheckLanes() const; // check if the goal is located behind the ego in the same route segment. 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 d9ff155f36e6c..a5aeb5b257d39 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 8406affd5529f..beeb675efd3ae 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_start_planner_module/src/manager.cpp @@ -49,11 +49,32 @@ void StartPlannerModuleManager::init(rclcpp::Node * node) p.collision_check_margin_from_front_object = node->declare_parameter(ns + "collision_check_margin_from_front_object"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + { + const std::string ns = "start_planner.object_types_to_check_for_path_generation."; + p.object_types_to_check_for_path_generation.check_car = + node->declare_parameter(ns + "check_car"); + p.object_types_to_check_for_path_generation.check_truck = + node->declare_parameter(ns + "check_truck"); + p.object_types_to_check_for_path_generation.check_bus = + node->declare_parameter(ns + "check_bus"); + p.object_types_to_check_for_path_generation.check_trailer = + node->declare_parameter(ns + "check_trailer"); + p.object_types_to_check_for_path_generation.check_unknown = + node->declare_parameter(ns + "check_unknown"); + p.object_types_to_check_for_path_generation.check_bicycle = + node->declare_parameter(ns + "check_bicycle"); + p.object_types_to_check_for_path_generation.check_motorcycle = + node->declare_parameter(ns + "check_motorcycle"); + p.object_types_to_check_for_path_generation.check_pedestrian = + node->declare_parameter(ns + "check_pedestrian"); + } p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = node->declare_parameter(ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + node->declare_parameter(ns + "allow_check_shift_path_lane_departure_override"); p.shift_collision_check_distance_from_end = node->declare_parameter(ns + "shift_collision_check_distance_from_end"); p.minimum_shift_pull_out_distance = @@ -334,15 +355,371 @@ void StartPlannerModuleManager::updateModuleParams( auto & p = parameters_; - [[maybe_unused]] const std::string ns = name_ + "."; + const std::string ns = "start_planner."; + + { + updateParam(parameters, ns + "th_arrived_distance", p->th_arrived_distance); + updateParam(parameters, ns + "th_stopped_velocity", p->th_stopped_velocity); + updateParam(parameters, ns + "th_stopped_time", p->th_stopped_time); + updateParam(parameters, ns + "prepare_time_before_start", p->prepare_time_before_start); + updateParam( + parameters, ns + "th_turn_signal_on_lateral_offset", p->th_turn_signal_on_lateral_offset); + updateParam( + parameters, ns + "th_distance_to_middle_of_the_road", p->th_distance_to_middle_of_the_road); + updateParam( + parameters, ns + "intersection_search_length", p->intersection_search_length); + updateParam( + parameters, ns + "length_ratio_for_turn_signal_deactivation_near_intersection", + p->length_ratio_for_turn_signal_deactivation_near_intersection); + updateParam>( + parameters, ns + "collision_check_margins", p->collision_check_margins); + updateParam( + parameters, ns + "collision_check_margin_from_front_object", + p->collision_check_margin_from_front_object); + updateParam(parameters, ns + "th_moving_object_velocity", p->th_moving_object_velocity); + const std::string obj_types_ns = ns + "object_types_to_check_for_path_generation."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->object_types_to_check_for_path_generation.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->object_types_to_check_for_path_generation.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->object_types_to_check_for_path_generation.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->object_types_to_check_for_path_generation.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->object_types_to_check_for_path_generation.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->object_types_to_check_for_path_generation.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->object_types_to_check_for_path_generation.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->object_types_to_check_for_path_generation.check_pedestrian); + } + updateParam(parameters, ns + "center_line_path_interval", p->center_line_path_interval); + updateParam(parameters, ns + "enable_shift_pull_out", p->enable_shift_pull_out); + updateParam( + parameters, ns + "shift_collision_check_distance_from_end", + p->shift_collision_check_distance_from_end); + updateParam( + parameters, ns + "minimum_shift_pull_out_distance", p->minimum_shift_pull_out_distance); + updateParam( + parameters, ns + "lateral_acceleration_sampling_num", p->lateral_acceleration_sampling_num); + updateParam(parameters, ns + "lateral_jerk", p->lateral_jerk); + updateParam(parameters, ns + "maximum_lateral_acc", p->maximum_lateral_acc); + updateParam(parameters, ns + "minimum_lateral_acc", p->minimum_lateral_acc); + updateParam(parameters, ns + "maximum_curvature", p->maximum_curvature); + updateParam(parameters, ns + "deceleration_interval", p->deceleration_interval); + updateParam(parameters, ns + "enable_geometric_pull_out", p->enable_geometric_pull_out); + updateParam(parameters, ns + "divide_pull_out_path", p->divide_pull_out_path); + updateParam( + parameters, ns + "arc_path_interval", p->parallel_parking_parameters.pull_out_path_interval); + updateParam( + parameters, ns + "lane_departure_margin", + p->parallel_parking_parameters.pull_out_lane_departure_margin); + updateParam( + parameters, ns + "pull_out_max_steer_angle", + p->parallel_parking_parameters.pull_out_max_steer_angle); + updateParam(parameters, ns + "enable_back", p->enable_back); + updateParam(parameters, ns + "backward_velocity", p->backward_velocity); + updateParam( + parameters, ns + "geometric_pull_out_velocity", + p->parallel_parking_parameters.pull_out_velocity); + updateParam( + parameters, ns + "geometric_collision_check_distance_from_end", + p->geometric_collision_check_distance_from_end); + updateParam( + parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure); + updateParam( + parameters, ns + "allow_check_shift_path_lane_departure_override", + p->allow_check_shift_path_lane_departure_override); + updateParam(parameters, ns + "search_priority", p->search_priority); + updateParam(parameters, ns + "max_back_distance", p->max_back_distance); + updateParam( + parameters, ns + "backward_search_resolution", p->backward_search_resolution); + updateParam( + parameters, ns + "backward_path_update_duration", p->backward_path_update_duration); + updateParam( + parameters, ns + "ignore_distance_from_lane_end", p->ignore_distance_from_lane_end); + } + { + const std::string ns = "start_planner.freespace_planner."; + + updateParam(parameters, ns + "enable_freespace_planner", p->enable_freespace_planner); + updateParam( + parameters, ns + "freespace_planner_algorithm", p->freespace_planner_algorithm); + updateParam( + parameters, ns + "end_pose_search_start_distance", p->end_pose_search_start_distance); + updateParam( + parameters, ns + "end_pose_search_end_distance", p->end_pose_search_end_distance); + updateParam(parameters, ns + "end_pose_search_interval", p->end_pose_search_interval); + updateParam(parameters, ns + "velocity", p->freespace_planner_velocity); + updateParam(parameters, ns + "vehicle_shape_margin", p->vehicle_shape_margin); + updateParam( + parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit); + updateParam( + parameters, ns + "minimum_turning_radius", + p->freespace_planner_common_parameters.minimum_turning_radius); + updateParam( + parameters, ns + "maximum_turning_radius", + p->freespace_planner_common_parameters.maximum_turning_radius); + updateParam( + parameters, ns + "turning_radius_size", + p->freespace_planner_common_parameters.turning_radius_size); + p->freespace_planner_common_parameters.maximum_turning_radius = std::max( + p->freespace_planner_common_parameters.maximum_turning_radius, + p->freespace_planner_common_parameters.minimum_turning_radius); + p->freespace_planner_common_parameters.turning_radius_size = + std::max(p->freespace_planner_common_parameters.turning_radius_size, 1); + } + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + + updateParam( + parameters, ns + "theta_size", p->freespace_planner_common_parameters.theta_size); + updateParam( + parameters, ns + "angle_goal_range", p->freespace_planner_common_parameters.angle_goal_range); + updateParam( + parameters, ns + "curve_weight", p->freespace_planner_common_parameters.curve_weight); + updateParam( + parameters, ns + "reverse_weight", p->freespace_planner_common_parameters.reverse_weight); + updateParam( + parameters, ns + "lateral_goal_range", + p->freespace_planner_common_parameters.lateral_goal_range); + updateParam( + parameters, ns + "longitudinal_goal_range", + p->freespace_planner_common_parameters.longitudinal_goal_range); + } + + { + const std::string ns = "start_planner.freespace_planner.costmap_configs."; + + updateParam( + parameters, ns + "obstacle_threshold", + p->freespace_planner_common_parameters.obstacle_threshold); + } + + { + const std::string ns = "start_planner.freespace_planner.astar."; + + updateParam(parameters, ns + "use_back", p->astar_parameters.use_back); + updateParam( + parameters, ns + "only_behind_solutions", p->astar_parameters.only_behind_solutions); + updateParam( + parameters, ns + "distance_heuristic_weight", p->astar_parameters.distance_heuristic_weight); + } + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + + updateParam(parameters, ns + "enable_update", p->rrt_star_parameters.enable_update); + updateParam( + parameters, ns + "use_informed_sampling", p->rrt_star_parameters.use_informed_sampling); + updateParam( + parameters, ns + "max_planning_time", p->rrt_star_parameters.max_planning_time); + updateParam(parameters, ns + "neighbor_radius", p->rrt_star_parameters.neighbor_radius); + updateParam(parameters, ns + "margin", p->rrt_star_parameters.margin); + } + + { + updateParam( + parameters, ns + "stop_condition.maximum_deceleration_for_stop", + p->maximum_deceleration_for_stop); + updateParam( + parameters, ns + "stop_condition.maximum_jerk_for_stop", p->maximum_jerk_for_stop); + } + + const std::string base_ns = "start_planner.path_safety_check."; + const std::string ego_path_ns = base_ns + "ego_predicted_path."; + + { + updateParam( + parameters, ego_path_ns + "min_velocity", p->ego_predicted_path_params.min_velocity); + updateParam( + parameters, ego_path_ns + "min_acceleration", p->ego_predicted_path_params.acceleration); + updateParam( + parameters, ego_path_ns + "time_horizon_for_front_object", + p->ego_predicted_path_params.time_horizon_for_front_object); + updateParam( + parameters, ego_path_ns + "time_horizon_for_rear_object", + p->ego_predicted_path_params.time_horizon_for_rear_object); + updateParam( + parameters, ego_path_ns + "time_resolution", p->ego_predicted_path_params.time_resolution); + updateParam( + parameters, ego_path_ns + "delay_until_departure", + p->ego_predicted_path_params.delay_until_departure); + } + + const std::string obj_filter_ns = base_ns + "target_filtering."; + + { + updateParam( + parameters, obj_filter_ns + "safety_check_time_horizon", + p->objects_filtering_params.safety_check_time_horizon); + updateParam( + parameters, obj_filter_ns + "safety_check_time_resolution", + p->objects_filtering_params.safety_check_time_resolution); + updateParam( + parameters, obj_filter_ns + "object_check_forward_distance", + p->objects_filtering_params.object_check_forward_distance); + updateParam( + parameters, obj_filter_ns + "object_check_backward_distance", + p->objects_filtering_params.object_check_backward_distance); + updateParam( + parameters, obj_filter_ns + "ignore_object_velocity_threshold", + p->objects_filtering_params.ignore_object_velocity_threshold); + updateParam( + parameters, obj_filter_ns + "include_opposite_lane", + p->objects_filtering_params.include_opposite_lane); + updateParam( + parameters, obj_filter_ns + "invert_opposite_lane", + p->objects_filtering_params.invert_opposite_lane); + updateParam( + parameters, obj_filter_ns + "check_all_predicted_path", + p->objects_filtering_params.check_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_all_predicted_path", + p->objects_filtering_params.use_all_predicted_path); + updateParam( + parameters, obj_filter_ns + "use_predicted_path_outside_lanelet", + p->objects_filtering_params.use_predicted_path_outside_lanelet); + } + + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; - std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { - if (!observer.expired()) { - const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); - if (start_planner_ptr) { - start_planner_ptr->updateModuleParams(p); - } + { + updateParam( + parameters, obj_types_ns + "check_car", + p->objects_filtering_params.object_types_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->objects_filtering_params.object_types_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->objects_filtering_params.object_types_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->objects_filtering_params.object_types_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->objects_filtering_params.object_types_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->objects_filtering_params.object_types_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->objects_filtering_params.object_types_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->objects_filtering_params.object_types_to_check.check_pedestrian); + } + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + + { + updateParam( + parameters, obj_lane_ns + "check_current_lane", + p->objects_filtering_params.object_lane_configuration.check_current_lane); + updateParam( + parameters, obj_lane_ns + "check_right_side_lane", + p->objects_filtering_params.object_lane_configuration.check_right_lane); + updateParam( + parameters, obj_lane_ns + "check_left_side_lane", + p->objects_filtering_params.object_lane_configuration.check_left_lane); + updateParam( + parameters, obj_lane_ns + "check_shoulder_lane", + p->objects_filtering_params.object_lane_configuration.check_shoulder_lane); + updateParam( + parameters, obj_lane_ns + "check_other_lane", + p->objects_filtering_params.object_lane_configuration.check_other_lane); + } + const std::string safety_check_ns = base_ns + "safety_check_params."; + { + updateParam( + parameters, safety_check_ns + "enable_safety_check", + p->safety_check_params.enable_safety_check); + updateParam( + parameters, safety_check_ns + "hysteresis_factor_expand_rate", + p->safety_check_params.hysteresis_factor_expand_rate); + updateParam( + parameters, safety_check_ns + "backward_path_length", + p->safety_check_params.backward_path_length); + updateParam( + parameters, safety_check_ns + "forward_path_length", + p->safety_check_params.forward_path_length); + updateParam( + parameters, safety_check_ns + "publish_debug_marker", + p->safety_check_params.publish_debug_marker); + } + const std::string rss_ns = safety_check_ns + "rss_params."; + { + updateParam( + parameters, rss_ns + "rear_vehicle_reaction_time", + p->safety_check_params.rss_params.rear_vehicle_reaction_time); + updateParam( + parameters, rss_ns + "rear_vehicle_safety_time_margin", + p->safety_check_params.rss_params.rear_vehicle_safety_time_margin); + updateParam( + parameters, rss_ns + "lateral_distance_max_threshold", + p->safety_check_params.rss_params.lateral_distance_max_threshold); + updateParam( + parameters, rss_ns + "longitudinal_distance_min_threshold", + p->safety_check_params.rss_params.longitudinal_distance_min_threshold); + updateParam( + parameters, rss_ns + "longitudinal_velocity_delta_time", + p->safety_check_params.rss_params.longitudinal_velocity_delta_time); + } + std::string surround_moving_obstacle_check_ns = ns + "surround_moving_obstacle_check."; + { + updateParam( + parameters, surround_moving_obstacle_check_ns + "search_radius", p->search_radius); + updateParam( + parameters, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity", + p->th_moving_obstacle_velocity); + + // ObjectTypesToCheck + std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + { + updateParam( + parameters, obj_types_ns + "check_car", + p->surround_moving_obstacles_type_to_check.check_car); + updateParam( + parameters, obj_types_ns + "check_truck", + p->surround_moving_obstacles_type_to_check.check_truck); + updateParam( + parameters, obj_types_ns + "check_bus", + p->surround_moving_obstacles_type_to_check.check_bus); + updateParam( + parameters, obj_types_ns + "check_trailer", + p->surround_moving_obstacles_type_to_check.check_trailer); + updateParam( + parameters, obj_types_ns + "check_unknown", + p->surround_moving_obstacles_type_to_check.check_unknown); + updateParam( + parameters, obj_types_ns + "check_bicycle", + p->surround_moving_obstacles_type_to_check.check_bicycle); + updateParam( + parameters, obj_types_ns + "check_motorcycle", + p->surround_moving_obstacles_type_to_check.check_motorcycle); + updateParam( + parameters, obj_types_ns + "check_pedestrian", + p->surround_moving_obstacles_type_to_check.check_pedestrian); } + } + + std::string debug_ns = ns + "debug."; + { + updateParam(parameters, debug_ns + "print_debug_info", p->print_debug_info); + } + + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { + if (!observer.expired()) observer.lock()->updateModuleParams(p); }); } 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 8eca8479fbd44..f5674cfb288d0 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 @@ -60,9 +60,8 @@ 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. - + // 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{}; { @@ -75,44 +74,44 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } + const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + // if lane departure check override is true, and if the initial pose is not fully within a lane, + // cancel lane departure check + const bool is_lane_departure_check_required = std::invoke([&]() -> bool { + if (!parameters_.allow_check_shift_path_lane_departure_override) + return parameters_.check_shift_path_lane_departure; + + PathWithLaneId path_with_only_first_pose{}; + path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.at(0)); + return !lane_departure_checker_->checkPathWillLeaveLane( + lanelet_map_ptr, path_with_only_first_pose); + }); + + // 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. + + if ( + is_lane_departure_check_required && + 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(departure_check_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 - // 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. - // TODO(someone): improve the method for detecting lane departures without using - // lanelet::ConstLanelets, making it unnecessary to retain departure_check_lanes_ as a member - // variable. - if ( - parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - departure_check_lanes_, path_shift_start_to_end)) { - continue; - } + const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( + lanelet_map_ptr, shift_path, start_segment_idx); + 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 acccd5319bab0..3c08d62f500ae 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 @@ -69,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"); @@ -142,7 +143,6 @@ void StartPlannerModule::initVariables() info_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(debug_data_.collision_check); - updateDepartureCheckLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -176,22 +176,20 @@ void StartPlannerModule::updateData() DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); } - if (!status_.first_approved_time && isActivated()) { - status_.first_approved_time = clock_->now(); + if ( + planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && + !status_.first_engaged_time) { + status_.first_engaged_time = clock_->now(); } - if (hasFinishedBackwardDriving()) { + status_.backward_driving_complete = hasFinishedBackwardDriving(); + if (status_.backward_driving_complete) { updateStatusAfterBackwardDriving(); DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving"); - } else { - status_.backward_driving_complete = false; } - if (requiresDynamicObjectsCollisionDetection()) { - status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects(); - } else { - status_.is_safe_dynamic_objects = true; - } + status_.is_safe_dynamic_objects = + (!requiresDynamicObjectsCollisionDetection()) ? true : !hasCollisionWithDynamicObjects(); } bool StartPlannerModule::hasFinishedBackwardDriving() const @@ -362,20 +360,17 @@ bool StartPlannerModule::isStopped() bool StartPlannerModule::isExecutionReady() const { - bool is_safe = true; // Evaluate safety. The situation is not safe if any of the following conditions are met: // 1. pull out path has not been found // 2. there is a moving objects around ego // 3. waiting for approval and there is a collision with dynamic objects - if (!status_.found_pull_out_path) { - is_safe = false; - } else if (isWaitingApproval()) { - if (!noMovingObjectsAround()) { - is_safe = false; - } else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) { - is_safe = false; - } - } + + const bool is_safe = [&]() -> bool { + if (!status_.found_pull_out_path) return false; + if (!isWaitingApproval()) return true; + if (!noMovingObjectsAround()) return false; + return !(requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()); + }(); if (!is_safe) { stop_pose_ = planner_data_->self_odometry->pose.pose; @@ -457,14 +452,7 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::STRAIGHT; - }); + const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( @@ -478,15 +466,16 @@ BehaviorModuleOutput StartPlannerModule::plan() {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); - } else { - const double distance = motion_utils::calcSignedArcLength( - path.points, planner_data_->self_odometry->pose.pose.position, - status_.pull_out_path.start_pose.position); - updateRTCStatus(0.0, distance); - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); + setDebugData(); + return output; } + const double distance = motion_utils::calcSignedArcLength( + path.points, planner_data_->self_odometry->pose.pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); setDebugData(); @@ -567,14 +556,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; - } - return SteeringFactor::STRAIGHT; - }); + const auto steering_factor_direction = getSteeringFactorDirection(output); if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( @@ -588,15 +570,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); - } else { - const double distance = motion_utils::calcSignedArcLength( - stop_path.points, planner_data_->self_odometry->pose.pose.position, - status_.pull_out_path.start_pose.position); - updateRTCStatus(0.0, distance); - steering_factor_interface_ptr_->updateSteeringFactor( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); + setDebugData(); + + return output; } + const double distance = motion_utils::calcSignedArcLength( + stop_path.points, planner_data_->self_odometry->pose.pose.position, + status_.pull_out_path.start_pose.position); + updateRTCStatus(0.0, distance); + steering_factor_interface_ptr_->updateSteeringFactor( + {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); setDebugData(); @@ -606,7 +590,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; - updateDepartureCheckLanes(); } void StartPlannerModule::incrementPathIndex() @@ -657,16 +640,20 @@ PriorityOrder StartPlannerModule::determinePriorityOrder( order_priority.emplace_back(i, planner); } } - } else if (search_priority == "short_back_distance") { + return order_priority; + } + + if (search_priority == "short_back_distance") { for (size_t i = 0; i < start_pose_candidates_num; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } } - } else { - RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); - throw std::domain_error("[start_planner] invalid search_priority"); + return order_priority; } + + RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); + throw std::domain_error("[start_planner] invalid search_priority"); return order_priority; } @@ -681,9 +668,10 @@ bool StartPlannerModule::findPullOutPath( // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_->th_moving_object_velocity); - const auto [pull_out_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + auto [pull_out_lane_stop_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); + utils::path_safety_checker::filterObjectsByClass( + pull_out_lane_stop_objects, parameters_->object_types_to_check_for_path_generation); // if start_pose_candidate is far from refined_start_pose, backward driving is necessary const bool backward_is_unnecessary = @@ -1049,6 +1037,9 @@ PredictedObjects StartPlannerModule::filterStopObjectsInPullOutLanes( stop_objects_in_pull_out_lanes, path.points, current_point, object_check_forward_distance, object_check_backward_distance); + utils::path_safety_checker::filterObjectsByClass( + stop_objects_in_pull_out_lanes, parameters_->object_types_to_check_for_path_generation); + return stop_objects_in_pull_out_lanes; } @@ -1084,11 +1075,11 @@ bool StartPlannerModule::hasFinishedPullOut() const bool StartPlannerModule::needToPrepareBlinkerBeforeStart() const { - if (!status_.first_approved_time) { + if (!status_.first_engaged_time) { return true; } - const auto first_approved_time = status_.first_approved_time.value(); - const double elapsed = rclcpp::Duration(clock_->now() - first_approved_time).seconds(); + const auto first_engaged_time = status_.first_engaged_time.value(); + const double elapsed = rclcpp::Duration(clock_->now() - first_engaged_time).seconds(); return elapsed < parameters_->prepare_time_before_start; } @@ -1098,12 +1089,7 @@ bool StartPlannerModule::isStuck() return false; } - if (status_.planner_type == PlannerType::STOP) { - return true; - } - - // not found safe path - if (!status_.found_pull_out_path) { + if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) { return true; } @@ -1196,30 +1182,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const return false; }); - 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; - } + turn_signal.required_end_point = std::invoke([&]() { + if (is_near_intersection) return end_pose; + 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; - } + } + // not found required end point + return end_pose; + }); return turn_signal; } @@ -1388,38 +1370,27 @@ bool StartPlannerModule::planFreespacePath() void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.planner_type == PlannerType::FREESPACE) { - 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; - } else { - const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - output.path, generateDrivableLanes(output.path), - planner_data_->drivable_area_expansion_parameters); - - DrivableAreaInfo current_drivable_area_info; - current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = - status_.driving_forward - ? utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) - : current_drivable_area_info; - } -} - -void StartPlannerModule::updateDepartureCheckLanes() -{ - const auto departure_check_lanes = createDepartureCheckLanes(); - for (auto & planner : start_planners_) { - auto shift_pull_out = std::dynamic_pointer_cast(planner); - - if (shift_pull_out) { - shift_pull_out->setDepartureCheckLanes(departure_check_lanes); + switch (status_.planner_type) { + case PlannerType::FREESPACE: { + 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; + break; + } + default: { + const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( + output.path, generateDrivableLanes(output.path), + planner_data_->drivable_area_expansion_parameters); + + DrivableAreaInfo current_drivable_area_info; + current_drivable_area_info.drivable_lanes = target_drivable_lanes; + output.drivable_area_info = + status_.driving_forward + ? utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) + : current_drivable_area_info; + break; } - } - // debug - { - debug_data_.departure_check_lanes = departure_check_lanes; } } diff --git a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt index 3217b3828b8d7..69dd1ced695ec 100644 --- a/planning/behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_crosswalk_module/CMakeLists.txt @@ -6,10 +6,8 @@ autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/manager.cpp - src/scene_crosswalk.cpp - src/util.cpp + DIRECTORY + src ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 4c7b214763c31..7d6acbfc6aa9a 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -193,6 +193,50 @@ document. | `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | | `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | +### Occlusion + +This feature makes ego slow down for a crosswalk that is occluded. + +Occlusion of the crosswalk is determined using the occupancy grid. +An occlusion is a square of size `min_size` of occluded cells +(i.e., their values are between `free_space_max` and `occupied_min`) +of size `min_size`. +If an occlusion is found within range of the crosswalk, +then the velocity limit at the crosswalk is set to `slow_down_velocity` (or more to not break limits set by `max_slow_down_jerk` and `max_slow_down_accel`). +The range is calculated from the intersection between the ego path and the crosswalk and is equal to the time taken by ego to reach the crosswalk times the `occluded_object_velocity`. +This range is meant to be large when ego is far from the crosswalk and small when ego is close. + +In order to avoid flickering decisions, a time buffer can be used such that the decision to add (or remove) the slow down is only taken +after an occlusion is detected (or not detected) for a consecutive time defined by the `time_buffer` parameter. + +To ignore occlusions when the pedestrian light is red, `ignore_with_red_traffic_light` should be set to true. + +To ignore temporary occlusions caused by moving objects, +`ignore_behind_predicted_objects` should be set to true. +By default, occlusions behind an object with velocity higher than `ignore_velocity_thresholds.default` are ignored. +This velocity threshold can be specified depending on the object type by specifying the object class label and velocity threshold in the parameter lists `ignore_velocity_thresholds.custom_labels` and `ignore_velocity_thresholds.custom_thresholds`. +To inflate the masking behind objects, their footprint can be made bigger using `extra_predicted_objects_size`. + +
+ ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} +
+ +| Parameter | Unit | Type | Description | +| ---------------------------------------------- | ----- | ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | +| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | +| `slow_down_velocity` | [m/s] | double | slow down velocity | +| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | +| `min_size` | [m] | double | minimum size of an occlusion (square side size) | +| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | +| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | +| `ignore_with_red_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | +| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | +| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) | +| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | +| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | + ### Others In the `common` namespace, the following parameters are defined. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index cf3d2c621322b..09ce64593ff46 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -68,3 +68,20 @@ bicycle: true # [-] whether to look and stop by BICYCLE objects motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects + + # param for occlusions + occlusion: + enable: true # if true, ego will slowdown around crosswalks that are occluded + occluded_object_velocity: 1.0 # [m/s] assumed velocity of objects that may come out of the occluded space + slow_down_velocity: 1.0 # [m/s] + time_buffer: 1.0 # [s] consecutive time with/without an occlusion to add/remove the slowdown + min_size: 0.5 # [m] minimum size of an occlusion (square side size) + free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid + occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid + ignore_with_red_traffic_light: true # [-] if true, occlusions at crosswalks with traffic lights are ignored + ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored + ignore_velocity_thresholds: + default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity + custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) + custom_thresholds: [0.0] # velocities of the custom labels + extra_predicted_objects_size: 0.5 # [m] extra size added to the objects for masking the occlusions diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg new file mode 100644 index 0000000000000..a98995e0da3a9 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/with_occlusion.svg @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ occluded area +
+
+
+ + + + + + + + + + + +
+
+
obstacle
+
+
+ + + + + + + diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 9eef11e72239e..2c38a9836e0af 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -28,9 +28,14 @@ behavior_velocity_planner_common eigen geometry_msgs + grid_map_core + grid_map_ros + grid_map_utils + interpolation lanelet2_extension libboost-dev motion_utils + nav_msgs pcl_conversions pluginlib rclcpp diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 1fa9247548add..39f88d5389c2b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -123,6 +124,46 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".object_filtering.target_object.motorcycle"); cp.look_pedestrian = getOrDeclareParameter(node, ns + ".object_filtering.target_object.pedestrian"); + + // param for occlusions + cp.occlusion_enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); + cp.occlusion_occluded_object_velocity = + getOrDeclareParameter(node, ns + ".occlusion.occluded_object_velocity"); + cp.occlusion_slow_down_velocity = + getOrDeclareParameter(node, ns + ".occlusion.slow_down_velocity"); + cp.occlusion_time_buffer = getOrDeclareParameter(node, ns + ".occlusion.time_buffer"); + cp.occlusion_min_size = getOrDeclareParameter(node, ns + ".occlusion.min_size"); + cp.occlusion_free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); + cp.occlusion_occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); + cp.occlusion_ignore_with_red_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.ignore_with_red_traffic_light"); + cp.occlusion_ignore_behind_predicted_objects = + getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); + + cp.occlusion_ignore_velocity_thresholds.resize( + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); + const auto get_label = [](const std::string & s) { + if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; + if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; + if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; + if (s == "MOTORCYCLE") + return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + if (s == "PEDESTRIAN") + return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + }; + const auto custom_labels = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); + const auto custom_velocities = getOrDeclareParameter>( + node, ns + ".occlusion.ignore_velocity_thresholds.custom_thresholds"); + for (auto idx = 0UL; idx < std::min(custom_labels.size(), custom_velocities.size()); ++idx) { + cp.occlusion_ignore_velocity_thresholds[get_label(custom_labels[idx])] = custom_velocities[idx]; + } + cp.occlusion_extra_objects_size = + getOrDeclareParameter(node, ns + ".occlusion.extra_predicted_objects_size"); } void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp new file mode 100644 index 0000000000000..2455b36d5883f --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -0,0 +1,173 @@ +// 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 "occluded_crosswalk.hpp" + +#include "behavior_velocity_crosswalk_module/util.hpp" + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +bool is_occluded( + const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) +{ + grid_map::Index idx_offset; + for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { + for (idx_offset.y() = 0; idx_offset.y() < min_nb_of_cells; ++idx_offset.y()) { + const auto index = idx + idx_offset; + if ((index < grid_map.getSize()).all()) { + const auto cell_value = grid_map.at("layer", index); + if ( + cell_value < params.occlusion_free_space_max || + cell_value > params.occlusion_occupied_min) + return false; + } + } + } + return true; +} + +lanelet::BasicPoint2d interpolate_point( + const lanelet::BasicSegment2d & segment, const double extra_distance) +{ + const auto direction_vector = (segment.second - segment.first).normalized(); + return segment.second + extra_distance * direction_vector; +} + +std::vector calculate_detection_areas( + const lanelet::ConstLanelet & crosswalk_lanelet, const lanelet::BasicPoint2d & crosswalk_origin, + const double detection_range) +{ + std::vector detection_areas = { + crosswalk_lanelet.polygon2d().basicPolygon()}; + const std::vector> + segment_getters = { + [](const auto & ls) -> lanelet::BasicSegment2d { + return {ls[1].basicPoint2d(), ls[0].basicPoint2d()}; + }, + [](const auto & ls) -> lanelet::BasicSegment2d { + return {ls[ls.size() - 2].basicPoint2d(), ls[ls.size() - 1].basicPoint2d()}; + }}; + if ( + crosswalk_lanelet.centerline2d().size() > 1 && crosswalk_lanelet.leftBound2d().size() > 1 && + crosswalk_lanelet.rightBound2d().size() > 1) { + for (const auto & segment_getter : segment_getters) { + const auto center_segment = segment_getter(crosswalk_lanelet.centerline2d()); + const auto left_segment = segment_getter(crosswalk_lanelet.leftBound2d()); + const auto right_segment = segment_getter(crosswalk_lanelet.rightBound2d()); + const auto dist = lanelet::geometry::distance2d(center_segment.second, crosswalk_origin); + if (dist < detection_range) { + const auto target_left = interpolate_point(left_segment, detection_range - dist); + const auto target_right = interpolate_point(right_segment, detection_range - dist); + detection_areas.push_back( + {left_segment.second, target_left, target_right, right_segment.second}); + } + } + } + return detection_areas; +} + +std::vector select_and_inflate_objects( + const std::vector & objects, + const std::vector velocity_thresholds, const double inflate_size) +{ + std::vector selected_objects; + for (const auto & o : objects) { + const auto vel_threshold = velocity_thresholds[o.classification.front().label]; + if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) { + auto selected_object = o; + selected_object.shape.dimensions.x += inflate_size; + selected_object.shape.dimensions.y += inflate_size; + selected_objects.push_back(selected_object); + } + } + return selected_objects; +} + +void clear_occlusions_behind_objects( + grid_map::GridMap & grid_map, + const std::vector & objects) +{ + const auto angle_cmp = [&](const auto & p1, const auto & p2) { + const auto d1 = p1 - grid_map.getPosition(); + const auto d2 = p2 - grid_map.getPosition(); + return std::atan2(d1.y(), d1.x()) < std::atan2(d2.y(), d2.x()); + }; + const lanelet::BasicPoint2d grid_map_position = grid_map.getPosition(); + const auto range = grid_map.getLength().maxCoeff() / 2.0; + for (auto object : objects) { + const lanelet::BasicPoint2d object_position = { + object.kinematics.initial_pose_with_covariance.pose.position.x, + object.kinematics.initial_pose_with_covariance.pose.position.y}; + if (lanelet::geometry::distance2d(grid_map_position, object_position) < range) { + lanelet::BasicPoints2d edge_points; + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + for (const auto & edge_point : object_polygon.outer()) edge_points.push_back(edge_point); + std::sort(edge_points.begin(), edge_points.end(), angle_cmp); + // points.push_back(interpolate_point({object_position, edge_point}, 10.0 * range)); + grid_map::Polygon polygon_to_clear; + polygon_to_clear.addVertex(edge_points.front()); + polygon_to_clear.addVertex( + interpolate_point({grid_map_position, edge_points.front()}, 10.0 * range)); + polygon_to_clear.addVertex( + interpolate_point({grid_map_position, edge_points.back()}, 10.0 * range)); + polygon_to_clear.addVertex(edge_points.back()); + for (grid_map_utils::PolygonIterator it(grid_map, polygon_to_clear); !it.isPastEnd(); ++it) + grid_map.at("layer", *it) = 0; + } + } +} + +bool is_crosswalk_occluded( + const lanelet::ConstLanelet & crosswalk_lanelet, + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const geometry_msgs::msg::Point & path_intersection, const double detection_range, + const std::vector & dynamic_objects, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) +{ + grid_map::GridMap grid_map; + grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); + + if (params.occlusion_ignore_behind_predicted_objects) { + const auto objects = select_and_inflate_objects( + dynamic_objects, params.occlusion_ignore_velocity_thresholds, + params.occlusion_extra_objects_size); + clear_occlusions_behind_objects(grid_map, objects); + } + const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution()); + for (const auto & detection_area : calculate_detection_areas( + crosswalk_lanelet, {path_intersection.x, path_intersection.y}, detection_range)) { + grid_map::Polygon poly; + for (const auto & p : detection_area) poly.addVertex(grid_map::Position(p.x(), p.y())); + for (grid_map_utils::PolygonIterator iter(grid_map, poly); !iter.isPastEnd(); ++iter) + if (is_occluded(grid_map, min_nb_of_cells, *iter, params)) return true; + } + return false; +} + +double calculate_detection_range( + const double occluded_object_velocity, const double dist_ego_to_crosswalk, + const double ego_velocity) +{ + constexpr double min_ego_velocity = 1.0; + const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); + return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp new file mode 100644 index 0000000000000..a76fdeb549b88 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -0,0 +1,94 @@ +// 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. + +#ifndef OCCLUDED_CROSSWALK_HPP_ +#define OCCLUDED_CROSSWALK_HPP_ + +#include "scene_crosswalk.hpp" + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +/// @brief check if the gridmap is occluded at the given index +/// @param [in] grid_map input grid map +/// @param [in] min_nb_of_cells minimum number of occluded cells needed to detect an occlusion (as +/// side of a square centered at the index) +/// @param [in] idx target index in the grid map +/// @param [in] params parameters +/// @return true if the index is occluded +bool is_occluded( + const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + +/// @brief interpolate a point beyond the end of the given segment +/// @param [in] segment input segment +/// @param [in] extra_distance desired distance beyond the end of the segment +/// @return interpolated point beyond the end of the segment +lanelet::BasicPoint2d interpolate_point( + const lanelet::BasicSegment2d & segment, const double extra_distance); + +/// @brief check if the crosswalk is occluded +/// @param crosswalk_lanelet lanelet of the crosswalk +/// @param occupancy_grid occupancy grid with the occlusion information +/// @param path_intersection intersection between the crosswalk and the ego path +/// @param detection_range range away from the crosswalk until occlusions are considered +/// @param dynamic_objects dynamic objects +/// @param params parameters +/// @return true if the crosswalk is occluded +bool is_crosswalk_occluded( + const lanelet::ConstLanelet & crosswalk_lanelet, + const nav_msgs::msg::OccupancyGrid & occupancy_grid, + const geometry_msgs::msg::Point & path_intersection, const double detection_range, + const std::vector & dynamic_objects, + const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + +/// @brief calculate the distance away from the crosswalk that should be checked for occlusions +/// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions +/// @param dist_ego_to_crosswalk distance between ego and the crosswalk +/// @param ego_velocity current velocity of ego +double calculate_detection_range( + const double occluded_object_velocity, const double dist_ego_to_crosswalk, + const double ego_velocity); + +/// @brief select a subset of objects meeting the velocity threshold and inflate their shape +/// @param objects input objects +/// @param velocity_threshold minimum velocity for an object to be selected +/// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities +/// @param inflate_size [m] size by which the shape of the selected objects are inflated +/// @return selected and inflated objects +std::vector select_and_inflate_objects( + const std::vector & objects, + const double velocity_threshold, const bool skip_pedestrians, const double inflate_size); + +/// @brief clear occlusions behind the given objects +/// @details masks behind the object assuming rays from the center of the grid map +/// @param grid_map grid map +/// @param objects objects +void clear_occlusions_behind_objects( + grid_map::GridMap & grid_map, + const std::vector & objects); +} // namespace behavior_velocity_planner + +#endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index cee6975df3155..00ace56f41d7c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2020-2023 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,6 +14,8 @@ #include "scene_crosswalk.hpp" +#include "occluded_crosswalk.hpp" + #include #include #include @@ -31,6 +33,7 @@ #include #include +#include #include #include #include @@ -228,7 +231,48 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto // Apply safety slow down speed if defined in Lanelet2 map if (crosswalk_.hasAttribute("safety_slow_down_speed")) { - applySafetySlowDownSpeed(*path, path_intersects); + applySafetySlowDownSpeed( + *path, path_intersects, + static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get())); + } + // Apply safety slow down speed if the crosswalk is occluded + const auto now = clock_->now(); + const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) { + return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer); + }; + const auto is_crosswalk_ignored = + (planner_param_.occlusion_ignore_with_red_traffic_light && isRedSignalForPedestrians()) || + crosswalk_.hasAttribute("skip_occluded_slowdown"); + if (planner_param_.occlusion_enable && !path_intersects.empty() && !is_crosswalk_ignored) { + const auto dist_ego_to_crosswalk = + calcSignedArcLength(path->points, ego_pos, path_intersects.front()); + const auto detection_range = + planner_data_->vehicle_info_.max_lateral_offset_m + + calculate_detection_range( + planner_param_.occlusion_occluded_object_velocity, dist_ego_to_crosswalk, + planner_data_->current_velocity->twist.linear.x); + const auto is_ego_on_the_crosswalk = + dist_ego_to_crosswalk <= planner_data_->vehicle_info_.max_longitudinal_offset_m; + if (!is_ego_on_the_crosswalk) { + if (is_crosswalk_occluded( + crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), detection_range, + objects_ptr->objects, planner_param_)) { + if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = now; + if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal{})) + most_recent_occlusion_time_ = now; + } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater{})) { + current_initial_occlusion_time_.reset(); + } + + if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal{})) { + const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path); + applySafetySlowDownSpeed( + *path, path_intersects, + std::max(target_velocity, planner_param_.occlusion_slow_down_velocity)); + } else { + most_recent_occlusion_time_.reset(); + } + } } recordTime(2); @@ -666,8 +710,6 @@ std::optional CrosswalkModule::getCollisionPoint( { stop_watch_.tic(__func__); - std::vector ret{}; - const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; const auto & ego_pos = planner_data_->current_odometry->pose.position; @@ -786,7 +828,8 @@ CollisionPoint CrosswalkModule::createCollisionPoint( } void CrosswalkModule::applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects) + PathWithLaneId & output, const std::vector & path_intersects, + const float safety_slow_down_speed) { if (path_intersects.empty()) { return; @@ -795,10 +838,6 @@ void CrosswalkModule::applySafetySlowDownSpeed( const auto & ego_pos = planner_data_->current_odometry->pose.position; const auto ego_path = output; - // Safety slow down speed in [m/s] - const auto & safety_slow_down_speed = - static_cast(crosswalk_.attribute("safety_slow_down_speed").asDouble().get()); - if (!passed_safety_slow_point_) { // Safety slow down distance [m] const double safety_slow_down_distance = @@ -1051,6 +1090,7 @@ void CrosswalkModule::updateObjectState( object.kinematics.initial_pose_with_covariance.pose, object.shape, getLabelColor(collision_state)); } + object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index d1196bb7b4a9a..5ce5d4020928a 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -154,6 +154,18 @@ class CrosswalkModule : public SceneModuleInterface bool look_bicycle; bool look_motorcycle; bool look_pedestrian; + // param for occlusions + bool occlusion_enable; + double occlusion_occluded_object_velocity; + float occlusion_slow_down_velocity; + double occlusion_time_buffer; + double occlusion_min_size; + int occlusion_free_space_max; + int occlusion_occupied_min; + bool occlusion_ignore_with_red_traffic_light; + bool occlusion_ignore_behind_predicted_objects; + std::vector occlusion_ignore_velocity_thresholds; + double occlusion_extra_objects_size; }; struct ObjectInfo @@ -309,7 +321,8 @@ class CrosswalkModule : public SceneModuleInterface private: // main functions void applySafetySlowDownSpeed( - PathWithLaneId & output, const std::vector & path_intersects); + PathWithLaneId & output, const std::vector & path_intersects, + const float speed); std::optional> getStopPointWithMargin( const PathWithLaneId & ego_path, @@ -430,6 +443,10 @@ class CrosswalkModule : public SceneModuleInterface // whether use regulatory element const bool use_regulatory_element_; + + // occluded space time buffer + std::optional current_initial_occlusion_time_; + std::optional most_recent_occlusion_time_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/behavior_velocity_intersection_module/src/decision_result.cpp index aefd59a72f9b4..7ed896d1b4b55 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.cpp @@ -24,36 +24,39 @@ std::string formatDecisionResult(const DecisionResult & decision_result) } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "OverPassJudge:\nsafety_report:" + state.safety_report + "\nevasive_report:\n" + - state.evasive_report; + return "OverPassJudge:\nsafety_report:" + state.safety_report + + "\nevasive_report:" + state.evasive_report; } if (std::holds_alternative(decision_result)) { return "StuckStop"; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "YieldStuckStop:\nsafety_report:" + state.safety_report; + return "YieldStuckStop:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "NonOccludedCollisionStop\nsafety_report:" + state.safety_report; + return "NonOccludedCollisionStop:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { - return "FirstWaitBeforeOcclusion"; + const auto & state = std::get(decision_result); + return "FirstWaitBeforeOcclusion:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { - return "PeekingTowardOcclusion"; + const auto & state = std::get(decision_result); + return "PeekingTowardOcclusion:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "OccludedCollisionStop\nsafety_report:" + state.safety_report; + return "OccludedCollisionStop:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); - return "OccludedAbsenceTrafficLight\nsafety_report:" + state.safety_report; + return "OccludedAbsenceTrafficLight:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { - return "Safe"; + const auto & state = std::get(decision_result); + return "Safe:\nocclusion_report:" + state.occlusion_report; } if (std::holds_alternative(decision_result)) { const auto & state = std::get(decision_result); diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/behavior_velocity_intersection_module/src/decision_result.hpp index da71168c2c4ca..5f642db3a462d 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/behavior_velocity_intersection_module/src/decision_result.hpp @@ -56,7 +56,7 @@ struct YieldStuckStop { size_t closest_idx{0}; size_t stuck_stopline_idx{0}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -67,7 +67,7 @@ struct NonOccludedCollisionStop size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -79,6 +79,7 @@ struct FirstWaitBeforeOcclusion size_t closest_idx{0}; size_t first_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string occlusion_report; }; /** @@ -98,6 +99,7 @@ struct PeekingTowardOcclusion //! contains the remaining time to release the static occlusion stuck and shows up //! intersection_occlusion(x.y) std::optional static_occlusion_timeout{std::nullopt}; + std::string occlusion_report; }; /** @@ -114,7 +116,7 @@ struct OccludedCollisionStop //! if null, it is dynamic occlusion and shows up intersection_occlusion(dyn). if valid, it //! contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -128,7 +130,7 @@ struct OccludedAbsenceTrafficLight size_t closest_idx{0}; size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; - std::string safety_report; + std::string occlusion_report; }; /** @@ -139,6 +141,7 @@ struct Safe size_t closest_idx{0}; size_t collision_stopline_idx{0}; size_t occlusion_stopline_idx{0}; + std::string occlusion_report; }; /** @@ -154,7 +157,7 @@ struct FullyPrioritized }; using DecisionResult = std::variant< - InternalError, //! internal process error, or over the pass judge line + InternalError, //! internal process error OverPassJudge, //! over the pass judge lines StuckStop, //! detected stuck vehicle YieldStuckStop, //! detected yield stuck vehicle diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index f82bbb0fbd5e6..3941362d96242 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -294,6 +294,11 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) } ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); + + decision_state_pub_ = + node.create_publisher("~/debug/intersection/decision_state", 1); + tl_observation_pub_ = node.create_publisher( + "~/debug/intersection_traffic_signal", 1); } void IntersectionModuleManager::launchNewModules( @@ -390,6 +395,10 @@ bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegister void IntersectionModuleManager::sendRTC(const Time & stamp) { + double min_distance = std::numeric_limits::infinity(); + std::optional nearest_tl_observation{std::nullopt}; + std_msgs::msg::String decision_type; + for (const auto & scene_module : scene_modules_) { const auto intersection_module = std::dynamic_pointer_cast(scene_module); const UUID uuid = getUUID(scene_module->getModuleId()); @@ -401,9 +410,27 @@ void IntersectionModuleManager::sendRTC(const Time & stamp) const auto occlusion_safety = intersection_module->getOcclusionSafety(); occlusion_rtc_interface_.updateCooperateStatus( occlusion_uuid, occlusion_safety, occlusion_distance, occlusion_distance, stamp); + + // ========================================================================================== + // module debug data + // ========================================================================================== + const auto internal_debug_data = intersection_module->getInternalDebugData(); + if (internal_debug_data.distance < min_distance) { + min_distance = internal_debug_data.distance; + nearest_tl_observation = internal_debug_data.tl_observation; + } + decision_type.data += (internal_debug_data.decision_type + "\n"); } rtc_interface_.publishCooperateStatus(stamp); // publishRTCStatus() occlusion_rtc_interface_.publishCooperateStatus(stamp); + + // ========================================================================================== + // publish module debug data + // ========================================================================================== + decision_state_pub_->publish(decision_type); + if (nearest_tl_observation) { + tl_observation_pub_->publish(nearest_tl_observation.value().signal); + } } void IntersectionModuleManager::setActivation() diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index 88af4412e34eb..46281df2f29c7 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -57,6 +58,9 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void setActivation() override; /* called from SceneModuleInterface::updateSceneModuleInstances */ void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + rclcpp::Publisher::SharedPtr decision_state_pub_; + rclcpp::Publisher::SharedPtr tl_observation_pub_; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index a6204ff218353..c2f78f28005dd 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -52,6 +52,7 @@ IntersectionModule::IntersectionModule( const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), + planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), turn_direction_(turn_direction), @@ -59,7 +60,6 @@ IntersectionModule::IntersectionModule( occlusion_uuid_(tier4_autoware_utils::generateUUID()) { velocity_factor_.init(PlanningBehavior::INTERSECTION); - planner_param_ = planner_param; { collision_state_machine_.setMarginTime( @@ -86,8 +86,6 @@ IntersectionModule::IntersectionModule( static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); } - decision_state_pub_ = - node.create_publisher("~/debug/intersection/decision_state", 1); ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); object_ttc_pub_ = node.create_publisher( @@ -107,9 +105,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * { const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + intersection::formatDecisionResult(decision_result); - std_msgs::msg::String decision_result_msg; - decision_result_msg.data = decision_type; - decision_state_pub_->publish(decision_result_msg); + internal_debug_data_.decision_type = decision_type; } prepareRTCStatus(decision_result, *path); @@ -130,6 +126,25 @@ void IntersectionModule::initializeRTCStatus() // activated_ and occlusion_activated_ must be set from manager's RTC callback } +static std::string formatOcclusionType(const IntersectionModule::OcclusionType & type) +{ + if (std::holds_alternative(type)) { + return "NotOccluded and the best occlusion clearance is " + + std::to_string(std::get(type).best_clearance_distance); + } + if (std::holds_alternative(type)) { + return "StaticallyOccluded and the best occlusion clearance is " + + std::to_string( + std::get(type).best_clearance_distance); + } + if (std::holds_alternative(type)) { + return "DynamicallyOccluded and the best occlusion clearance is " + + std::to_string( + std::get(type).best_clearance_distance); + } + return "RTCOccluded"; +} + intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -239,6 +254,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( detectCollision(is_over_1st_pass_judge_line, is_over_2nd_pass_judge_line); const std::string safety_diag = generateDetectionBlameDiagnosis(too_late_detect_objects, misjudge_objects); + const std::string occlusion_diag = formatOcclusionType(occlusion_status); + if (is_permanent_go_) { if (has_collision) { const auto closest_idx = intersection_stoplines.closest_idx; @@ -287,7 +304,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( isYieldStuckStatus(*path, interpolated_path_info, intersection_stoplines); if (is_yield_stuck_status) { auto yield_stuck = is_yield_stuck_status.value(); - yield_stuck.safety_report = safety_report; + yield_stuck.occlusion_report = occlusion_diag; return yield_stuck; } @@ -305,12 +322,13 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // Safe if (!is_occlusion_state && !has_collision_with_margin) { - return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + return intersection::Safe{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Only collision if (!is_occlusion_state && has_collision_with_margin) { return intersection::NonOccludedCollisionStop{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Occluded // utility functions @@ -384,7 +402,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, first_attention_stopline_idx, occlusion_wo_tl_pass_judge_line_idx, - safety_report}; + occlusion_diag}; } // ========================================================================================== @@ -407,7 +425,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool release_static_occlusion_stuck = (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); if (!has_collision_with_margin && release_static_occlusion_stuck) { - return intersection::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; + return intersection::Safe{ + closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED const double max_timeout = @@ -428,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_stopline_idx, occlusion_stopline_idx, static_occlusion_timeout, - safety_report}; + occlusion_diag}; } else { return intersection::PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, @@ -437,7 +456,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_stopline_idx, first_attention_stopline_idx, occlusion_stopline_idx, - static_occlusion_timeout}; + static_occlusion_timeout, + occlusion_diag}; } } else { const auto occlusion_stopline = @@ -445,7 +465,8 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( ? first_attention_stopline_idx : occlusion_stopline_idx; return intersection::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; + is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline, + occlusion_diag}; } } @@ -1254,6 +1275,7 @@ void IntersectionModule::updateTrafficSignalObservation() return; } last_tl_valid_observation_ = tl_info_opt.value(); + internal_debug_data_.tl_observation = tl_info_opt.value(); } IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c9a10cc8ba5b9..8917b4bac579b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -28,7 +28,6 @@ #include #include -#include #include #include @@ -242,6 +241,13 @@ class IntersectionModule : public SceneModuleInterface traffic_light_observation{std::nullopt}; }; + struct InternalDebugData + { + double distance{0.0}; + std::string decision_type{}; + std::optional tl_observation{std::nullopt}; + }; + using TimeDistanceArray = std::vector>; /** @@ -326,6 +332,7 @@ class IntersectionModule : public SceneModuleInterface double getOcclusionDistance() const { return occlusion_stop_distance_; } void setOcclusionActivation(const bool activation) { occlusion_activated_ = activation; } bool isOcclusionFirstStopRequired() const { return occlusion_first_stop_required_; } + InternalDebugData & getInternalDebugData() const { return internal_debug_data_; } private: /** @@ -336,6 +343,9 @@ class IntersectionModule : public SceneModuleInterface * following variables are unique to this intersection lanelet or to this module * @{ */ + + const PlannerParam planner_param_; + //! lanelet of this intersection const lanelet::Id lane_id_; @@ -361,7 +371,6 @@ class IntersectionModule : public SceneModuleInterface * following variables are immutable once initialized * @{ */ - PlannerParam planner_param_; //! cache IntersectionLanelets struct std::optional intersection_lanelets_{std::nullopt}; @@ -807,7 +816,7 @@ class IntersectionModule : public SceneModuleInterface /** @} */ mutable DebugData debug_data_; - rclcpp::Publisher::SharedPtr decision_state_pub_; + mutable InternalDebugData internal_debug_data_{}; rclcpp::Publisher::SharedPtr ego_ttc_pub_; rclcpp::Publisher::SharedPtr object_ttc_pub_; }; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 1ffdb75204e4f..52c6b06541796 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -189,6 +189,12 @@ IntersectionModule::prepareIntersectionData(const bool is_prioritized, PathWithL "Path has no interval on intersection lane " + std::to_string(lane_id_)); } + const auto & path_ip = interpolated_path_info.path; + const auto & path_ip_intersection_end = interpolated_path_info.lane_id_interval.value().second; + internal_debug_data_.distance = motion_utils::calcSignedArcLength( + path->points, current_pose.position, + path_ip.points.at(path_ip_intersection_end).point.pose.position); + if (!intersection_lanelets_) { intersection_lanelets_ = generateObjectiveLanelets(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index e049d02ffe9b5..92afd25026a70 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -324,9 +324,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } ++ego_area_start_idx; } - if (ego_area_start_idx > num_ignore_nearest) { - ego_area_start_idx--; - } + if (!is_in_area) { return ego_area; } @@ -338,6 +336,11 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( const auto & p = pp.at(i).point.pose.position; if (!bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(no_stopping_area).basicPolygon())) { dist_from_area_sum += tier4_autoware_utils::calcDistance2d(pp.at(i), pp.at(i - 1)); + + // do not take extra distance and exit as soon as p is outside no stopping area + // just a temporary fix + ego_area_end_idx = i - 1; + break; } if (dist_from_start_sum > extra_dist || dist_from_area_sum > margin) { break; diff --git a/planning/behavior_velocity_occlusion_spot_module/README.md b/planning/behavior_velocity_occlusion_spot_module/README.md index b0b774defbdf8..36b620df6567b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/README.md +++ b/planning/behavior_velocity_occlusion_spot_module/README.md @@ -119,11 +119,11 @@ The maximum slowdown velocity is calculated from the below parameters of ego cur | `use_object_info` | bool | [-] whether to reflect object info to occupancy grid map or not. | | `use_partition_lanelet` | bool | [-] whether to use partition lanelet map data. | -| Parameter /debug | Type | Description | -| ------------------------- | ---- | ---------------------------------------------- | -| `is_show_occlusion` | bool | [-] whether to show occlusion point markers.　 | -| `is_show_cv_window` | bool | [-] whether to show open_cv debug window. | -| `is_show_processing_time` | bool | [-] whether to show processing time. | +| Parameter /debug | Type | Description | +| ------------------------- | ---- | -------------------------------------------- | +| `is_show_occlusion` | bool | [-] whether to show occlusion point markers. | +| `is_show_cv_window` | bool | [-] whether to show open_cv debug window. | +| `is_show_processing_time` | bool | [-] whether to show processing time. | | Parameter /threshold | Type | Description | | ----------------------- | ------ | --------------------------------------------------------- | diff --git a/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json index a119139a13a20..67c9c06104b40 100644 --- a/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json +++ b/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json @@ -16,6 +16,11 @@ "default": "5.0", "description": "backward path" }, + "behavior_output_path_interval": { + "type": "number", + "default": "1.0", + "description": "the output path will be interpolated by this interval" + }, "max_accel": { "type": "number", "default": "-2.8", @@ -49,6 +54,7 @@ }, "required": [ "forward_path_length", + "behavior_output_path_interval", "backward_path_length", "max_accel", "system_delay", diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 9fa5634c6dd65..854f3c8852c29 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -146,6 +146,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. 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 239abbde27609..935530b52e175 100644 --- a/planning/behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/behavior_velocity_planner/test/src/test_node_interface.cpp @@ -79,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 606e41ad4b1d1..1511d430ddd0c 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 @@ -83,6 +83,10 @@ struct PlannerData std::optional external_velocity_limit; 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 diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 96cb2c4e1c790..413ce78beacee 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -281,15 +281,24 @@ bool TrafficLightModule::isStopSignal() { updateTrafficSignal(); - // If it never receives traffic signal, it will PASS. + // If there is no upcoming traffic signal information, + // SIMULATION: it will PASS to prevent stopping on the planning simulator + // or scenario simulator. + // REAL ENVIRONMENT: it will STOP for safety in cases such that traffic light + // recognition is not working properly or the map is incorrect. if (!traffic_signal_stamp_) { - return false; + if (planner_data_->is_simulation) { + return false; + } + return true; } + // Stop if the traffic signal information has timed out if (isTrafficSignalTimedOut()) { return true; } + // Check if the current traffic signal state requires stopping return traffic_light_utils::isTrafficSignalStop(lane_, looking_tl_state_); } diff --git a/planning/mission_planner/CMakeLists.txt b/planning/mission_planner/CMakeLists.txt index f5677f2681767..5f7e9666e03c0 100644 --- a/planning/mission_planner/CMakeLists.txt +++ b/planning/mission_planner/CMakeLists.txt @@ -14,13 +14,20 @@ rclcpp_components_register_node(goal_pose_visualizer_component ) ament_auto_add_library(${PROJECT_NAME}_component SHARED - src/mission_planner/mission_planner.cpp src/mission_planner/arrival_checker.cpp + src/mission_planner/service_utils.cpp + src/mission_planner/mission_planner.cpp + src/mission_planner/route_selector.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_component PLUGIN "mission_planner::MissionPlanner" - EXECUTABLE ${PROJECT_NAME} + EXECUTABLE mission_planner +) + +rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "mission_planner::RouteSelector" + EXECUTABLE route_selector ) ament_auto_add_library(${PROJECT_NAME}_lanelet2_plugins SHARED @@ -30,7 +37,7 @@ ament_auto_add_library(${PROJECT_NAME}_lanelet2_plugins SHARED pluginlib_export_plugin_description_file(mission_planner plugins/plugin_description.xml) ament_auto_package( - INSTALL_TO_SHARE - config - launch + INSTALL_TO_SHARE + config + launch ) diff --git a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp b/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp deleted file mode 100644 index ddd0bfa652f0e..0000000000000 --- a/planning/mission_planner/include/mission_planner/mission_planner_interface.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ -#define MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace mission_planner -{ - -struct SetMrmRoute -{ - using Service = autoware_adapi_v1_msgs::srv::SetRoutePoints; - static constexpr char name[] = "~/srv/set_mrm_route"; -}; - -struct ClearMrmRoute -{ - using Service = autoware_adapi_v1_msgs::srv::ClearRoute; - static constexpr char name[] = "~/srv/clear_mrm_route"; -}; - -struct ModifiedGoal -{ - using Message = autoware_planning_msgs::msg::PoseWithUuidStamped; - static constexpr char name[] = "input/modified_goal"; - static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; - static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; -}; - -} // namespace mission_planner - -#endif // MISSION_PLANNER__MISSION_PLANNER_INTERFACE_HPP_ diff --git a/planning/mission_planner/launch/mission_planner.launch.xml b/planning/mission_planner/launch/mission_planner.launch.xml index 013516a18f813..eca6d05bb9463 100644 --- a/planning/mission_planner/launch/mission_planner.launch.xml +++ b/planning/mission_planner/launch/mission_planner.launch.xml @@ -4,10 +4,24 @@ - - - - - - + + + + + + + + + + + + + + + + + + + + diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index b882a4c1da42e..b8ad634c9a0d4 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -18,10 +18,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_planning_msgs + autoware_auto_mapping_msgs autoware_planning_msgs - component_interface_specs - component_interface_utils geometry_msgs lanelet2_extension motion_utils @@ -29,7 +27,6 @@ rclcpp rclcpp_components route_handler - std_srvs tf2_geometry_msgs tf2_ros tier4_autoware_utils diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index bd5c78aba131e..4bc0f21dd947b 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -150,7 +150,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_goal_footprint_marker_ = - node_->create_publisher("debug/goal_footprint", durable_qos); + node_->create_publisher("~/debug/goal_footprint", durable_qos); vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); @@ -164,7 +164,7 @@ void DefaultPlanner::initialize(rclcpp::Node * node) { initialize_common(node); map_subscriber_ = node_->create_subscription( - "input/vector_map", rclcpp::QoS{10}.transient_local(), + "~/input/vector_map", rclcpp::QoS{10}.transient_local(), std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index e0181065150fd..e9a0108cdaa24 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -14,59 +14,20 @@ #include "mission_planner.hpp" +#include "service_utils.hpp" + #include #include #include #include -#include -#include #include #include #include #include -#include -#include - -namespace -{ - -using autoware_adapi_v1_msgs::msg::RoutePrimitive; -using autoware_adapi_v1_msgs::msg::RouteSegment; -using autoware_planning_msgs::msg::LaneletPrimitive; -using autoware_planning_msgs::msg::LaneletSegment; - -LaneletPrimitive convert(const RoutePrimitive & in) -{ - LaneletPrimitive out; - out.id = in.id; - out.primitive_type = in.type; - return out; -} - -LaneletSegment convert(const RouteSegment & in) -{ - LaneletSegment out; - out.primitives.reserve(in.alternatives.size() + 1); - out.primitives.push_back(convert(in.preferred)); - for (const auto & primitive : in.alternatives) { - out.primitives.push_back(convert(primitive)); - } - out.preferred_primitive = convert(in.preferred); - return out; -} - -std::array generate_random_id() -{ - static std::independent_bits_engine engine(std::random_device{}()); - std::array id; - std::generate(id.begin(), id.end(), std::ref(engine)); - return id; -} - -} // namespace +#include namespace mission_planner { @@ -78,11 +39,11 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) tf_buffer_(get_clock()), tf_listener_(tf_buffer_), odometry_(nullptr), - map_ptr_(nullptr), - reroute_availability_(nullptr), - normal_route_(nullptr), - mrm_route_(nullptr) + map_ptr_(nullptr) { + using std::placeholders::_1; + using std::placeholders::_2; + map_frame_ = declare_parameter("map_frame"); reroute_time_threshold_ = declare_parameter("reroute_time_threshold"); minimum_reroute_length_ = declare_parameter("minimum_reroute_length"); @@ -90,76 +51,76 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) planner_ = plugin_loader_.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); planner_->initialize(this); - sub_odometry_ = create_subscription( - "/localization/kinematic_state", rclcpp::QoS(1), - std::bind(&MissionPlanner::on_odometry, this, std::placeholders::_1)); - sub_reroute_availability_ = create_subscription( - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/" - "is_reroute_available", - rclcpp::QoS(1), - std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); + const auto reroute_availability = std::make_shared(); + reroute_availability->availability = false; + reroute_availability_ = reroute_availability; const auto durable_qos = rclcpp::QoS(1).transient_local(); + sub_odometry_ = create_subscription( + "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); sub_vector_map_ = create_subscription( - "input/vector_map", durable_qos, - std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); + "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); + sub_reroute_availability_ = create_subscription( + "~/input/reroute_availability", rclcpp::QoS(1), + std::bind(&MissionPlanner::on_reroute_availability, this, _1)); + pub_marker_ = create_publisher("~/debug/route_marker", durable_qos); + + // NOTE: The route interface should be mutually exclusive by callback group. sub_modified_goal_ = create_subscription( - "input/modified_goal", durable_qos, - std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1)); - - pub_marker_ = create_publisher("debug/route_marker", durable_qos); - - const auto adaptor = component_interface_utils::NodeAdaptor(this); - adaptor.init_pub(pub_state_); - adaptor.init_pub(pub_route_); - adaptor.init_pub(pub_normal_route_); - adaptor.init_pub(pub_mrm_route_); - adaptor.init_srv(srv_clear_route_, this, &MissionPlanner::on_clear_route); - adaptor.init_srv(srv_set_route_, this, &MissionPlanner::on_set_route); - adaptor.init_srv(srv_set_route_points_, this, &MissionPlanner::on_set_route_points); - adaptor.init_srv(srv_change_route_, this, &MissionPlanner::on_change_route); - adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); - adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); - adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); + "~/input/modified_goal", durable_qos, std::bind(&MissionPlanner::on_modified_goal, this, _1)); + srv_clear_route = create_service( + "~/clear_route", service_utils::handle_exception(&MissionPlanner::on_clear_route, this)); + srv_set_lanelet_route = create_service( + "~/set_lanelet_route", + service_utils::handle_exception(&MissionPlanner::on_set_lanelet_route, this)); + srv_set_waypoint_route = create_service( + "~/set_waypoint_route", + service_utils::handle_exception(&MissionPlanner::on_set_waypoint_route, this)); + pub_route_ = create_publisher("~/route", durable_qos); + pub_state_ = create_publisher("~/state", durable_qos); // Route state will be published when the node gets ready for route api after initialization, // otherwise the mission planner rejects the request for the API. - data_check_timer_ = create_wall_timer( - std::chrono::milliseconds(100), std::bind(&MissionPlanner::checkInitialization, this)); + const auto period = rclcpp::Rate(10).period(); + data_check_timer_ = create_wall_timer(period, [this] { check_initialization(); }); logger_configure_ = std::make_unique(this); } -void MissionPlanner::checkInitialization() +void MissionPlanner::check_initialization() { + auto logger = get_logger(); + auto clock = *get_clock(); + if (!planner_->ready()) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting lanelet map... Route API is not ready."); + RCLCPP_INFO_THROTTLE(logger, clock, 5000, "waiting lanelet map... Route API is not ready."); return; } if (!odometry_) { - RCLCPP_INFO_THROTTLE( - get_logger(), *get_clock(), 5000, "waiting odometry... Route API is not ready."); + RCLCPP_INFO_THROTTLE(logger, clock, 5000, "waiting odometry... Route API is not ready."); return; } // All data is ready. Now API is available. - RCLCPP_DEBUG(get_logger(), "Route API is ready."); - change_state(RouteState::Message::UNSET); - data_check_timer_->cancel(); // stop timer callback + RCLCPP_DEBUG(logger, "Route API is ready."); + change_state(RouteState::UNSET); + + // Stop timer callback. + data_check_timer_->cancel(); + data_check_timer_ = nullptr; } void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) { odometry_ = msg; - // NOTE: Do not check in the changing state as goal may change. - if (state_.state == RouteState::Message::SET) { + // NOTE: Do not check in the other states as goal may change. + if (state_.state == RouteState::SET) { PoseStamped pose; pose.header = odometry_->header; pose.pose = odometry_->pose.pose; if (arrival_checker_.is_arrived(pose)) { - change_state(RouteState::Message::ARRIVED); + change_state(RouteState::ARRIVED); } } } @@ -172,565 +133,273 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) { map_ptr_ = msg; + lanelet_map_ptr_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg(*map_ptr_, lanelet_map_ptr_); } -PoseStamped MissionPlanner::transform_pose(const PoseStamped & input) +Pose MissionPlanner::transform_pose(const Pose & pose, const Header & header) { - PoseStamped output; geometry_msgs::msg::TransformStamped transform; + geometry_msgs::msg::Pose result; try { - transform = tf_buffer_.lookupTransform(map_frame_, input.header.frame_id, tf2::TimePointZero); - tf2::doTransform(input, output, transform); - return output; + transform = tf_buffer_.lookupTransform(map_frame_, header.frame_id, tf2::TimePointZero); + tf2::doTransform(pose, result, transform); + return result; } catch (tf2::TransformException & error) { - throw component_interface_utils::TransformError(error.what()); - } -} - -void MissionPlanner::clear_route() -{ - arrival_checker_.set_goal(); - planner_->clearRoute(); - normal_route_ = nullptr; - // TODO(Takagi, Isamu): publish an empty route here -} - -void MissionPlanner::clear_mrm_route() -{ - mrm_route_ = nullptr; -} - -void MissionPlanner::change_route(const LaneletRoute & route) -{ - PoseWithUuidStamped goal; - goal.header = route.header; - goal.pose = route.goal_pose; - goal.uuid = route.uuid; - - arrival_checker_.set_goal(goal); - pub_route_->publish(route); - pub_normal_route_->publish(route); - pub_marker_->publish(planner_->visualize(route)); - planner_->updateRoute(route); - - // update normal route - normal_route_ = std::make_shared(route); -} - -void MissionPlanner::change_mrm_route(const LaneletRoute & route) -{ - PoseWithUuidStamped goal; - goal.header = route.header; - goal.pose = route.goal_pose; - goal.uuid = route.uuid; - - arrival_checker_.set_goal(goal); - pub_route_->publish(route); - pub_mrm_route_->publish(route); - pub_marker_->publish(planner_->visualize(route)); - planner_->updateRoute(route); - - // update emergency route - mrm_route_ = std::make_shared(route); -} - -LaneletRoute MissionPlanner::create_route( - const std_msgs::msg::Header & header, - const std::vector & route_segments, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) -{ - PoseStamped goal_pose_stamped; - goal_pose_stamped.header = header; - goal_pose_stamped.pose = goal_pose; - - // Convert route. - LaneletRoute route; - route.start_pose = odometry_->pose.pose; - route.goal_pose = transform_pose(goal_pose_stamped).pose; - for (const auto & segment : route_segments) { - route.segments.push_back(convert(segment)); - } - route.header.stamp = header.stamp; - route.header.frame_id = map_frame_; - route.uuid.uuid = generate_random_id(); - route.allow_modification = allow_goal_modification; - - return route; -} - -LaneletRoute MissionPlanner::create_route( - const std_msgs::msg::Header & header, const std::vector & waypoints, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification) -{ - // Use temporary pose stamped for transform. - PoseStamped pose; - pose.header = header; - - // Convert route points. - PlannerPlugin::RoutePoints points; - points.push_back(odometry_->pose.pose); - for (const auto & waypoint : waypoints) { - pose.pose = waypoint; - points.push_back(transform_pose(pose).pose); + throw service_utils::TransformError(error.what()); } - pose.pose = goal_pose; - points.push_back(transform_pose(pose).pose); - - // Plan route. - LaneletRoute route = planner_->plan(points); - route.header.stamp = header.stamp; - route.header.frame_id = map_frame_; - route.uuid.uuid = generate_random_id(); - route.allow_modification = allow_goal_modification; - - return route; -} - -LaneletRoute MissionPlanner::create_route(const SetRoute::Service::Request::SharedPtr req) -{ - const auto & header = req->header; - const auto & route_segments = req->segments; - const auto & goal_pose = req->goal; - const auto & allow_goal_modification = req->option.allow_goal_modification; - - return create_route(header, route_segments, goal_pose, allow_goal_modification); -} - -LaneletRoute MissionPlanner::create_route(const SetRoutePoints::Service::Request::SharedPtr req) -{ - const auto & header = req->header; - const auto & waypoints = req->waypoints; - const auto & goal_pose = req->goal; - const auto & allow_goal_modification = req->option.allow_goal_modification; - - return create_route(header, waypoints, goal_pose, allow_goal_modification); } -void MissionPlanner::change_state(RouteState::Message::_state_type state) +void MissionPlanner::change_state(RouteState::_state_type state) { state_.stamp = now(); state_.state = state; pub_state_->publish(state_); } -// NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_clear_route( - const ClearRoute::Service::Request::SharedPtr, const ClearRoute::Service::Response::SharedPtr res) -{ - clear_route(); - change_state(RouteState::Message::UNSET); - res->status.success = true; -} - -// NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_set_route( - const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res) +void MissionPlanner::on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; + RCLCPP_INFO(get_logger(), "Received modified goal."); - if (state_.state != RouteState::Message::UNSET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_ROUTE_EXISTS, "The route is already set."); + if (state_.state != RouteState::SET) { + RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); + return; } if (!planner_->ready()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); + RCLCPP_ERROR(get_logger(), "The planner is not ready."); + return; } if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); + RCLCPP_ERROR(get_logger(), "The vehicle pose is not received."); + return; } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + if (!current_route_) { + RCLCPP_ERROR(get_logger(), "The route has not set yet."); + return; + } + if (current_route_->uuid != msg->uuid) { + RCLCPP_ERROR(get_logger(), "Goal uuid is incorrect."); + return; } - // Convert request to a new route. - const auto route = create_route(req); + change_state(RouteState::REROUTING); + const auto route = create_route(*msg); - // Check planned routes if (route.segments.empty()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); + cancel_route(); + change_state(RouteState::SET); + RCLCPP_ERROR(get_logger(), "The planned route is empty."); } - // Update route. change_route(route); - change_state(RouteState::Message::SET); + change_state(RouteState::SET); + RCLCPP_INFO(get_logger(), "Changed the route with the modified goal"); +} + +void MissionPlanner::on_clear_route( + const ClearRoute::Request::SharedPtr, const ClearRoute::Response::SharedPtr res) +{ + change_route(); + change_state(RouteState::UNSET); res->status.success = true; } -// NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_set_route_points( - const SetRoutePoints::Service::Request::SharedPtr req, - const SetRoutePoints::Service::Response::SharedPtr res) +void MissionPlanner::on_set_lanelet_route( + const SetLaneletRoute::Request::SharedPtr req, const SetLaneletRoute::Response::SharedPtr res) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; + const auto is_reroute = state_.state == RouteState::SET; - if (state_.state != RouteState::Message::UNSET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_ROUTE_EXISTS, "The route is already set."); + if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); } if (!planner_->ready()) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); } if (!odometry_) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); + if (is_reroute && !reroute_availability_->availability) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } - // Plan route. - const auto route = create_route(req); + change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); + const auto route = create_route(*req); - // Check planned routes if (route.segments.empty()) { - throw component_interface_utils::ServiceException( + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - // Update route. + if (is_reroute && !check_reroute_safety(*current_route_, route)) { + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( + ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); + } + change_route(route); - change_state(RouteState::Message::SET); + change_state(RouteState::SET); res->status.success = true; } -// NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_set_mrm_route( - const SetMrmRoute::Service::Request::SharedPtr req, - const SetMrmRoute::Service::Response::SharedPtr res) +void MissionPlanner::on_set_waypoint_route( + const SetWaypointRoute::Request::SharedPtr req, const SetWaypointRoute::Response::SharedPtr res) { using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + const auto is_reroute = state_.state == RouteState::SET; + if (state_.state != RouteState::UNSET && state_.state != RouteState::SET) { + throw service_utils::ServiceException( + ResponseCode::ERROR_INVALID_STATE, "The route cannot be set in the current state."); + } if (!planner_->ready()) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); } if (!odometry_) { - throw component_interface_utils::ServiceException( + throw service_utils::ServiceException( ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); } - if (reroute_availability_ && !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( + if (is_reroute && !reroute_availability_->availability) { + throw service_utils::ServiceException( ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); } - const auto prev_state = state_.state; - change_state(RouteState::Message::CHANGING); + change_state(is_reroute ? RouteState::REROUTING : RouteState::ROUTING); + const auto route = create_route(*req); - // Plan route. - const auto new_route = create_route(req); - - if (new_route.segments.empty()) { - change_state(prev_state); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "Failed to plan a new route."); - } - - // check route safety - // step1. if in mrm state, check with mrm route - if (mrm_route_) { - if (check_reroute_safety(*mrm_route_, new_route)) { - // success to reroute - change_mrm_route(new_route); - res->status.success = true; - } else { - // failed to reroute - change_mrm_route(*mrm_route_); - res->status.success = false; - } - change_state(RouteState::Message::SET); - RCLCPP_INFO(get_logger(), "Route is successfully changed with the modified goal"); - return; + if (route.segments.empty()) { + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( + ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); } - if (!normal_route_) { - // if it does not set normal route, just use the new planned route - change_mrm_route(new_route); - change_state(RouteState::Message::SET); - res->status.success = true; - RCLCPP_INFO(get_logger(), "MRM route is successfully changed with the modified goal"); - return; + if (is_reroute && !check_reroute_safety(*current_route_, route)) { + cancel_route(); + change_state(is_reroute ? RouteState::SET : RouteState::UNSET); + throw service_utils::ServiceException( + ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); } - // step2. if not in mrm state, check with normal route - if (check_reroute_safety(*normal_route_, new_route)) { - // success to reroute - change_mrm_route(new_route); - res->status.success = true; - } else { - // Failed to reroute - change_route(*normal_route_); - res->status.success = false; - } - change_state(RouteState::Message::SET); + change_route(route); + change_state(RouteState::SET); + res->status.success = true; } -// NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_clear_mrm_route( - const ClearMrmRoute::Service::Request::SharedPtr, - const ClearMrmRoute::Service::Response::SharedPtr res) +void MissionPlanner::change_route() { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; - - if (!planner_->ready()) { - change_state(RouteState::Message::SET); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); - } - if (!mrm_route_) { - throw component_interface_utils::NoEffectWarning("MRM route is not set"); - } - if ( - state_.state == RouteState::Message::SET && reroute_availability_ && - !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, - "Cannot clear MRM route as the planner is not lane following before arriving at the goal."); - } - - change_state(RouteState::Message::CHANGING); - - if (!normal_route_) { - clear_mrm_route(); - change_state(RouteState::Message::UNSET); - res->status.success = true; - return; - } - - // check route safety - if (check_reroute_safety(*mrm_route_, *normal_route_)) { - clear_mrm_route(); - change_route(*normal_route_); - change_state(RouteState::Message::SET); - res->status.success = true; - return; - } - - // reroute with normal goal - const std::vector empty_waypoints; - const auto new_route = create_route( - odometry_->header, empty_waypoints, normal_route_->goal_pose, - normal_route_->allow_modification); + current_route_ = nullptr; + planner_->clearRoute(); + arrival_checker_.set_goal(); - // check new route safety - if (new_route.segments.empty() || !check_reroute_safety(*mrm_route_, new_route)) { - // failed to create a new route - RCLCPP_ERROR(get_logger(), "Reroute with normal goal failed."); - change_mrm_route(*mrm_route_); - change_state(RouteState::Message::SET); - res->status.success = false; - } else { - clear_mrm_route(); - change_route(new_route); - change_state(RouteState::Message::SET); - res->status.success = true; - } + // TODO(Takagi, Isamu): publish an empty route here + // pub_route_->publish(); + // pub_marker_->publish(); } -void MissionPlanner::on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg) +void MissionPlanner::change_route(const LaneletRoute & route) { - RCLCPP_INFO(get_logger(), "Received modified goal."); - - if (state_.state != RouteState::Message::SET) { - RCLCPP_ERROR(get_logger(), "The route hasn't set yet. Cannot reroute."); - return; - } - if (!planner_->ready()) { - RCLCPP_ERROR(get_logger(), "The planner is not ready."); - return; - } - if (!odometry_) { - RCLCPP_ERROR(get_logger(), "The vehicle pose is not received."); - return; - } - if (!normal_route_) { - RCLCPP_ERROR(get_logger(), "Normal route has not set yet."); - return; - } - - if (mrm_route_ && mrm_route_->uuid == msg->uuid) { - // set to changing state - change_state(RouteState::Message::CHANGING); - - const std::vector empty_waypoints; - auto new_route = - create_route(msg->header, empty_waypoints, msg->pose, mrm_route_->allow_modification); - // create_route generate new uuid, so set the original uuid again to keep that. - new_route.uuid = msg->uuid; - if (new_route.segments.empty()) { - change_mrm_route(*mrm_route_); - change_state(RouteState::Message::SET); - RCLCPP_ERROR(get_logger(), "The planned MRM route is empty."); - return; - } - - change_mrm_route(new_route); - change_state(RouteState::Message::SET); - RCLCPP_INFO(get_logger(), "Changed the MRM route with the modified goal"); - return; - } - - if (normal_route_->uuid == msg->uuid) { - // set to changing state - change_state(RouteState::Message::CHANGING); - - const std::vector empty_waypoints; - auto new_route = - create_route(msg->header, empty_waypoints, msg->pose, normal_route_->allow_modification); - // create_route generate new uuid, so set the original uuid again to keep that. - new_route.uuid = msg->uuid; - if (new_route.segments.empty()) { - change_route(*normal_route_); - change_state(RouteState::Message::SET); - RCLCPP_ERROR(get_logger(), "The planned route is empty."); - return; - } + PoseWithUuidStamped goal; + goal.header = route.header; + goal.pose = route.goal_pose; + goal.uuid = route.uuid; - change_route(new_route); - change_state(RouteState::Message::SET); - RCLCPP_INFO(get_logger(), "Changed the route with the modified goal"); - return; - } + current_route_ = std::make_shared(route); + planner_->updateRoute(route); + arrival_checker_.set_goal(goal); - RCLCPP_ERROR(get_logger(), "Goal uuid is incorrect."); + pub_route_->publish(route); + pub_marker_->publish(planner_->visualize(route)); } -void MissionPlanner::on_change_route( - const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res) +void MissionPlanner::cancel_route() { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoute::Response; - - if (state_.state != RouteState::Message::SET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); - } - if (!planner_->ready()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); - } - if (!normal_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); - } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); - } - if (reroute_availability_ && !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); + // Restore planner state that changes with create_route function. + if (current_route_) { + planner_->updateRoute(*current_route_); } +} - // set to changing state - change_state(RouteState::Message::CHANGING); +LaneletRoute MissionPlanner::create_route(const SetLaneletRoute::Request & req) +{ + const auto & header = req.header; + const auto & segments = req.segments; + const auto & goal_pose = req.goal_pose; + const auto & uuid = req.uuid; + const auto & allow_goal_modification = req.allow_modification; - // Convert request to a new route. - const auto new_route = create_route(req); + return create_route(header, segments, goal_pose, uuid, allow_goal_modification); +} - // Check planned routes - if (new_route.segments.empty()) { - change_route(*normal_route_); - change_state(RouteState::Message::SET); - res->status.success = false; - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); - } +LaneletRoute MissionPlanner::create_route(const SetWaypointRoute::Request & req) +{ + const auto & header = req.header; + const auto & waypoints = req.waypoints; + const auto & goal_pose = req.goal_pose; + const auto & uuid = req.uuid; + const auto & allow_goal_modification = req.allow_modification; - // check route safety - if (check_reroute_safety(*normal_route_, new_route)) { - // success to reroute - change_route(new_route); - res->status.success = true; - change_state(RouteState::Message::SET); - } else { - // failed to reroute - change_route(*normal_route_); - res->status.success = false; - change_state(RouteState::Message::SET); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); - } + return create_route(header, waypoints, goal_pose, uuid, allow_goal_modification); } -// NOTE: The route interface should be mutually exclusive by callback group. -void MissionPlanner::on_change_route_points( - const SetRoutePoints::Service::Request::SharedPtr req, - const SetRoutePoints::Service::Response::SharedPtr res) +LaneletRoute MissionPlanner::create_route(const PoseWithUuidStamped & msg) { - using ResponseCode = autoware_adapi_v1_msgs::srv::SetRoutePoints::Response; + const auto & header = msg.header; + const auto & goal_pose = msg.pose; + const auto & uuid = msg.uuid; + const auto & allow_goal_modification = current_route_->allow_modification; - if (state_.state != RouteState::Message::SET) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "The route hasn't set yet. Cannot reroute."); - } - if (!planner_->ready()) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The planner is not ready."); - } - if (!odometry_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "The vehicle pose is not received."); - } - if (!normal_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_UNREADY, "Normal route is not set."); - } - if (mrm_route_) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute in the emergency state."); - } - if (reroute_availability_ && !reroute_availability_->availability) { - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_INVALID_STATE, "Cannot reroute as the planner is not in lane following."); - } - - change_state(RouteState::Message::CHANGING); + return create_route(header, std::vector(), goal_pose, uuid, allow_goal_modification); +} - // Plan route. - const auto new_route = create_route(req); +LaneletRoute MissionPlanner::create_route( + const Header & header, const std::vector & segments, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification) +{ + LaneletRoute route; + route.header.stamp = header.stamp; + route.header.frame_id = map_frame_; + route.start_pose = odometry_->pose.pose; + route.goal_pose = transform_pose(goal_pose, header); + route.segments = segments; + route.uuid = uuid; + route.allow_modification = allow_goal_modification; + return route; +} - // Check planned routes - if (new_route.segments.empty()) { - change_state(RouteState::Message::SET); - change_route(*normal_route_); - res->status.success = false; - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_PLANNER_FAILED, "The planned route is empty."); +LaneletRoute MissionPlanner::create_route( + const Header & header, const std::vector & waypoints, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification) +{ + PlannerPlugin::RoutePoints points; + points.push_back(odometry_->pose.pose); + for (const auto & waypoint : waypoints) { + points.push_back(transform_pose(waypoint, header)); } + points.push_back(transform_pose(goal_pose, header)); - // check route safety - if (check_reroute_safety(*normal_route_, new_route)) { - // success to reroute - change_route(new_route); - res->status.success = true; - change_state(RouteState::Message::SET); - } else { - // failed to reroute - change_route(*normal_route_); - res->status.success = false; - change_state(RouteState::Message::SET); - throw component_interface_utils::ServiceException( - ResponseCode::ERROR_REROUTE_FAILED, "New route is not safe. Reroute failed."); - } + LaneletRoute route = planner_->plan(points); + route.header.stamp = header.stamp; + route.header.frame_id = map_frame_; + route.uuid = uuid; + route.allow_modification = allow_goal_modification; + return route; } bool MissionPlanner::check_reroute_safety( const LaneletRoute & original_route, const LaneletRoute & target_route) { - if (original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || !odometry_) { + if ( + original_route.segments.empty() || target_route.segments.empty() || !map_ptr_ || + !lanelet_map_ptr_ || !odometry_) { RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Route, map or odometry is not set."); return false; } @@ -749,112 +418,135 @@ bool MissionPlanner::check_reroute_safety( return false; } - bool is_same = false; for (const auto & primitive : original_primitives) { const auto has_same = [&](const auto & p) { return p.id == primitive.id; }; - is_same = std::find_if(target_primitives.begin(), target_primitives.end(), has_same) != - target_primitives.end(); - } - return is_same; - }; - - // find idx of original primitives that matches the target primitives - const auto start_idx_opt = std::invoke([&]() -> std::optional { - /* - * find the index of the original route that has same idx with the front segment of the new - * route - * - * start_idx - * +-----------+-----------+-----------+-----------+-----------+ - * | | | | | | - * +-----------+-----------+-----------+-----------+-----------+ - * | | | | | | - * +-----------+-----------+-----------+-----------+-----------+ - * original original original original original - * target target target - */ - const auto target_front_primitives = target_route.segments.front().primitives; - for (size_t i = 0; i < original_route.segments.size(); ++i) { - const auto & original_primitives = original_route.segments.at(i).primitives; - if (hasSamePrimitives(original_primitives, target_front_primitives)) { - return i; + const bool is_same = + std::find_if(target_primitives.begin(), target_primitives.end(), has_same) != + target_primitives.end(); + if (!is_same) { + return false; } } + return true; + }; - /* - * find the target route that has same idx with the front segment of the original route - * - * start_idx - * +-----------+-----------+-----------+-----------+-----------+ - * | | | | | | - * +-----------+-----------+-----------+-----------+-----------+ - * | | | | | | - * +-----------+-----------+-----------+-----------+-----------+ - * 　　　　　　　　　　　　　　　original original original - * target target target target target - */ - const auto original_front_primitives = original_route.segments.front().primitives; - for (size_t i = 0; i < target_route.segments.size(); ++i) { - const auto & target_primitives = target_route.segments.at(i).primitives; - if (hasSamePrimitives(target_primitives, original_front_primitives)) { - return 0; + // ============================================================================================= + // NOTE: the target route is calculated while ego is driving on the original route, so basically + // the first lane of the target route should be in the original route lanelets. So the common + // segment interval matches the beginning of the target route. The exception is that if ego is + // on an intersection lanelet, getClosestLanelet() may not return the same lanelet which exists + // in the original route. In that case the common segment interval does not match the beginning of + // the target lanelet + // ============================================================================================= + const auto start_idx_opt = + std::invoke([&]() -> std::optional> { + for (size_t i = 0; i < original_route.segments.size(); ++i) { + const auto & original_segment = original_route.segments.at(i).primitives; + for (size_t j = 0; j < target_route.segments.size(); ++j) { + const auto & target_segment = target_route.segments.at(j).primitives; + if (hasSamePrimitives(original_segment, target_segment)) { + return std::make_pair(i, j); + } + } } - } - - return std::nullopt; - }); + return std::nullopt; + }); if (!start_idx_opt.has_value()) { RCLCPP_ERROR( get_logger(), "Check reroute safety failed. Cannot find the start index of the route."); return false; } - const size_t start_idx = start_idx_opt.value(); + const auto [start_idx_original, start_idx_target] = start_idx_opt.value(); // find last idx that matches the target primitives - size_t end_idx = start_idx; - for (size_t i = 1; i < target_route.segments.size(); ++i) { - if (start_idx + i > original_route.segments.size() - 1) { + size_t end_idx_original = start_idx_original; + size_t end_idx_target = start_idx_target; + for (size_t i = 1; i < target_route.segments.size() - start_idx_target; ++i) { + if (start_idx_original + i > original_route.segments.size() - 1) { break; } - const auto & original_primitives = original_route.segments.at(start_idx + i).primitives; - const auto & target_primitives = target_route.segments.at(i).primitives; + const auto & original_primitives = + original_route.segments.at(start_idx_original + i).primitives; + const auto & target_primitives = target_route.segments.at(start_idx_target + i).primitives; if (!hasSamePrimitives(original_primitives, target_primitives)) { break; } - end_idx = start_idx + i; + end_idx_original = start_idx_original + i; + end_idx_target = start_idx_target + i; + } + + // at the very first transition from main/MRM to MRM/main, the requested route from the + // route_selector may not begin from ego current lane (because route_selector requests the + // previous route once, and then replan) + const bool ego_is_on_first_target_section = std::any_of( + target_route.segments.front().primitives.begin(), + target_route.segments.front().primitives.end(), [&](const auto & primitive) { + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id); + return lanelet::utils::isInLanelet(target_route.start_pose, lanelet); + }); + if (!ego_is_on_first_target_section) { + RCLCPP_ERROR( + get_logger(), + "Check reroute safety failed. Ego is not on the first section of target route."); + return false; } - // create map - auto lanelet_map_ptr_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*map_ptr_, lanelet_map_ptr_); + // if the front of target route is not the front of common segment, it is expected that the front + // of the target route is conflicting with another lane which is equal to original + // route[start_idx_original-1] + double accumulated_length = 0.0; - // compute distance from the current pose to the end of the current lanelet - const auto current_pose = target_route.start_pose; - const auto primitives = original_route.segments.at(start_idx).primitives; - lanelet::ConstLanelets start_lanelets; - for (const auto & primitive : primitives) { - const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id); - start_lanelets.push_back(lanelet); - } + if (start_idx_target != 0 && start_idx_original > 1) { + // compute distance from the current pose to the beginning of the common segment + const auto current_pose = target_route.start_pose; + const auto primitives = original_route.segments.at(start_idx_original - 1).primitives; + lanelet::ConstLanelets start_lanelets; + for (const auto & primitive : primitives) { + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id); + start_lanelets.push_back(lanelet); + } + // closest lanelet in start lanelets + lanelet::ConstLanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet."); + return false; + } - // get closest lanelet in start lanelets - lanelet::ConstLanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) { - RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet."); - return false; - } + const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline()); + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + const double dist_to_current_pose = arc_coordinates.length; + const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet); + accumulated_length = lanelet_length - dist_to_current_pose; + } else { + // compute distance from the current pose to the end of the current lanelet + const auto current_pose = target_route.start_pose; + const auto primitives = original_route.segments.at(start_idx_original).primitives; + lanelet::ConstLanelets start_lanelets; + for (const auto & primitive : primitives) { + const auto lanelet = lanelet_map_ptr_->laneletLayer.get(primitive.id); + start_lanelets.push_back(lanelet); + } + // closest lanelet in start lanelets + lanelet::ConstLanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLanelet(start_lanelets, current_pose, &closest_lanelet)) { + RCLCPP_ERROR(get_logger(), "Check reroute safety failed. Cannot find the closest lanelet."); + return false; + } - const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline()); - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); - const auto arc_coordinates = lanelet::geometry::toArcCoordinates( - centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - const double dist_to_current_pose = arc_coordinates.length; - const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet); - double accumulated_length = lanelet_length - dist_to_current_pose; + const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline()); + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(current_pose.position); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + const double dist_to_current_pose = arc_coordinates.length; + const double lanelet_length = lanelet::utils::getLaneletLength2d(closest_lanelet); + accumulated_length = lanelet_length - dist_to_current_pose; + } // compute distance from the start_idx+1 to end_idx - for (size_t i = start_idx + 1; i <= end_idx; ++i) { + for (size_t i = start_idx_original + 1; i <= end_idx_original; ++i) { const auto primitives = original_route.segments.at(i).primitives; if (primitives.empty()) { break; @@ -870,7 +562,7 @@ bool MissionPlanner::check_reroute_safety( } // check if the goal is inside of the target terminal lanelet - const auto & target_end_primitives = target_route.segments.at(end_idx - start_idx).primitives; + const auto & target_end_primitives = target_route.segments.at(end_idx_target).primitives; const auto & target_goal = target_route.goal_pose; for (const auto & target_end_primitive : target_end_primitives) { const auto lanelet = lanelet_map_ptr_->laneletLayer.get(target_end_primitive.id); @@ -882,8 +574,11 @@ bool MissionPlanner::check_reroute_safety( lanelet::utils::to2D(target_goal_position).basicPoint()) .length; const double target_lanelet_length = lanelet::utils::getLaneletLength2d(lanelet); - const double dist = target_lanelet_length - dist_to_goal; - accumulated_length = std::max(accumulated_length - dist, 0.0); + // NOTE: `accumulated_length` here contains the length of the entire target_end_primitive, so + // the remaining distance from the goal to the end of the target_end_primitive needs to be + // subtracted. + const double remaining_dist = target_lanelet_length - dist_to_goal; + accumulated_length = std::max(accumulated_length - remaining_dist, 0.0); break; } } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index ea44d2643e186..8016851d5a7d3 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -16,19 +16,22 @@ #define MISSION_PLANNER__MISSION_PLANNER_HPP_ #include "arrival_checker.hpp" -#include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include -#include #include #include #include #include +#include +#include +#include #include #include #include +#include +#include +#include +#include #include #include @@ -41,23 +44,22 @@ namespace mission_planner { -using PoseStamped = geometry_msgs::msg::PoseStamped; -using PoseWithUuidStamped = autoware_planning_msgs::msg::PoseWithUuidStamped; -using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; -using LaneletPrimitive = autoware_planning_msgs::msg::LaneletPrimitive; -using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; -using MarkerArray = visualization_msgs::msg::MarkerArray; -using ClearRoute = planning_interface::ClearRoute; -using SetRoutePoints = planning_interface::SetRoutePoints; -using SetRoute = planning_interface::SetRoute; -using ChangeRoutePoints = planning_interface::ChangeRoutePoints; -using ChangeRoute = planning_interface::ChangeRoute; -using Route = planning_interface::Route; -using NormalRoute = planning_interface::NormalRoute; -using MrmRoute = planning_interface::MrmRoute; -using RouteState = planning_interface::RouteState; -using Odometry = nav_msgs::msg::Odometry; -using RerouteAvailability = tier4_planning_msgs::msg::RerouteAvailability; +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::PoseWithUuidStamped; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; +using std_msgs::msg::Header; +using tier4_planning_msgs::msg::RerouteAvailability; +using tier4_planning_msgs::msg::RouteState; +using tier4_planning_msgs::srv::ClearRoute; +using tier4_planning_msgs::srv::SetLaneletRoute; +using tier4_planning_msgs::srv::SetWaypointRoute; +using unique_identifier_msgs::msg::UUID; +using visualization_msgs::msg::MarkerArray; class MissionPlanner : public rclcpp::Node { @@ -68,86 +70,64 @@ class MissionPlanner : public rclcpp::Node ArrivalChecker arrival_checker_; pluginlib::ClassLoader plugin_loader_; std::shared_ptr planner_; + std::string map_frame_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - PoseStamped transform_pose(const PoseStamped & input); + Pose transform_pose(const Pose & pose, const Header & header); + + rclcpp::Service::SharedPtr srv_clear_route; + rclcpp::Service::SharedPtr srv_set_lanelet_route; + rclcpp::Service::SharedPtr srv_set_waypoint_route; + rclcpp::Publisher::SharedPtr pub_state_; + rclcpp::Publisher::SharedPtr pub_route_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; - rclcpp::Subscription::SharedPtr sub_modified_goal_; + rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; HADMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; + RouteState state_; + LaneletRoute::ConstSharedPtr current_route_; + lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; + void on_odometry(const Odometry::ConstSharedPtr msg); void on_map(const HADMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); - rclcpp::TimerBase::SharedPtr data_check_timer_; - void checkInitialization(); - - rclcpp::Publisher::SharedPtr pub_marker_; - void clear_route(); - void clear_mrm_route(); + void on_clear_route( + const ClearRoute::Request::SharedPtr req, const ClearRoute::Response::SharedPtr res); + void on_set_lanelet_route( + const SetLaneletRoute::Request::SharedPtr req, const SetLaneletRoute::Response::SharedPtr res); + void on_set_waypoint_route( + const SetWaypointRoute::Request::SharedPtr req, + const SetWaypointRoute::Response::SharedPtr res); + + void change_state(RouteState::_state_type state); + void change_route(); void change_route(const LaneletRoute & route); - void change_mrm_route(const LaneletRoute & route); - LaneletRoute create_route(const SetRoute::Service::Request::SharedPtr req); - LaneletRoute create_route(const SetRoutePoints::Service::Request::SharedPtr req); + void cancel_route(); + LaneletRoute create_route(const SetLaneletRoute::Request & req); + LaneletRoute create_route(const SetWaypointRoute::Request & req); + LaneletRoute create_route(const PoseWithUuidStamped & msg); LaneletRoute create_route( - const std_msgs::msg::Header & header, - const std::vector & route_segments, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); + const Header & header, const std::vector & segments, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification); LaneletRoute create_route( - const std_msgs::msg::Header & header, const std::vector & waypoints, - const geometry_msgs::msg::Pose & goal_pose, const bool allow_goal_modification); - - RouteState::Message state_; - component_interface_utils::Publisher::SharedPtr pub_state_; - component_interface_utils::Publisher::SharedPtr pub_route_; - component_interface_utils::Publisher::SharedPtr pub_normal_route_; - component_interface_utils::Publisher::SharedPtr pub_mrm_route_; - void change_state(RouteState::Message::_state_type state); - - component_interface_utils::Service::SharedPtr srv_clear_route_; - component_interface_utils::Service::SharedPtr srv_set_route_; - component_interface_utils::Service::SharedPtr srv_set_route_points_; - component_interface_utils::Service::SharedPtr srv_change_route_; - component_interface_utils::Service::SharedPtr srv_change_route_points_; - void on_clear_route( - const ClearRoute::Service::Request::SharedPtr req, - const ClearRoute::Service::Response::SharedPtr res); - void on_set_route( - const SetRoute::Service::Request::SharedPtr req, - const SetRoute::Service::Response::SharedPtr res); - void on_set_route_points( - const SetRoutePoints::Service::Request::SharedPtr req, - const SetRoutePoints::Service::Response::SharedPtr res); - - component_interface_utils::Service::SharedPtr srv_set_mrm_route_; - component_interface_utils::Service::SharedPtr srv_clear_mrm_route_; - void on_set_mrm_route( - const SetMrmRoute::Service::Request::SharedPtr req, - const SetMrmRoute::Service::Response::SharedPtr res); - void on_clear_mrm_route( - const ClearMrmRoute::Service::Request::SharedPtr req, - const ClearMrmRoute::Service::Response::SharedPtr res); - - void on_change_route( - const SetRoute::Service::Request::SharedPtr req, - const SetRoute::Service::Response::SharedPtr res); - void on_change_route_points( - const SetRoutePoints::Service::Request::SharedPtr req, - const SetRoutePoints::Service::Response::SharedPtr res); - - double reroute_time_threshold_{10.0}; - double minimum_reroute_length_{30.0}; - bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); + const Header & header, const std::vector & waypoints, const Pose & goal_pose, + const UUID & uuid, const bool allow_goal_modification); - std::shared_ptr normal_route_{nullptr}; - std::shared_ptr mrm_route_{nullptr}; + rclcpp::TimerBase::SharedPtr data_check_timer_; + void check_initialization(); + + double reroute_time_threshold_; + double minimum_reroute_length_; + bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); std::unique_ptr logger_configure_; }; diff --git a/planning/mission_planner/src/mission_planner/route_selector.cpp b/planning/mission_planner/src/mission_planner/route_selector.cpp new file mode 100644 index 0000000000000..7547333d55cd1 --- /dev/null +++ b/planning/mission_planner/src/mission_planner/route_selector.cpp @@ -0,0 +1,302 @@ +// 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 "route_selector.hpp" + +#include "service_utils.hpp" + +#include +#include +#include + +namespace mission_planner::uuid +{ + +std::array generate_random_id() +{ + static std::independent_bits_engine engine(std::random_device{}()); + std::array id; + std::generate(id.begin(), id.end(), std::ref(engine)); + return id; +} + +UUID generate_if_empty(const UUID & uuid) +{ + constexpr std::array zero_uuid = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + + UUID result; + result.uuid = (uuid.uuid == zero_uuid) ? generate_random_id() : uuid.uuid; + return result; +} + +} // namespace mission_planner::uuid + +namespace mission_planner +{ + +RouteInterface::RouteInterface(rclcpp::Clock::SharedPtr clock) +{ + clock_ = clock; + state_.state = RouteState::UNKNOWN; +} + +RouteState::_state_type RouteInterface::get_state() const +{ + return state_.state; +} + +std::optional RouteInterface::get_route() const +{ + return route_; +} + +void RouteInterface::change_route() +{ + route_ = std::nullopt; +} + +void RouteInterface::change_state(RouteState::_state_type state) +{ + state_.stamp = clock_->now(); + state_.state = state; + pub_state_->publish(state_); +} + +void RouteInterface::update_state(const RouteState & state) +{ + state_ = state; + pub_state_->publish(state_); +} + +void RouteInterface::update_route(const LaneletRoute & route) +{ + route_ = route; + pub_route_->publish(route); +} + +RouteSelector::RouteSelector(const rclcpp::NodeOptions & options) +: Node("route_selector", options), main_(get_clock()), mrm_(get_clock()) +{ + using std::placeholders::_1; + using std::placeholders::_2; + const auto service_qos = rmw_qos_profile_services_default; + const auto durable_qos = rclcpp::QoS(1).transient_local(); + + // Init main route interface. + main_.srv_clear_route = create_service( + "~/main/clear_route", + service_utils::handle_exception(&RouteSelector::on_clear_route_main, this)); + main_.srv_set_waypoint_route = create_service( + "~/main/set_waypoint_route", + service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_main, this)); + main_.srv_set_lanelet_route = create_service( + "~/main/set_lanelet_route", + service_utils::handle_exception(&RouteSelector::on_set_lanelet_route_main, this)); + main_.pub_state_ = create_publisher("~/main/state", durable_qos); + main_.pub_route_ = create_publisher("~/main/route", durable_qos); + + // Init mrm route interface. + mrm_.srv_clear_route = create_service( + "~/mrm/clear_route", service_utils::handle_exception(&RouteSelector::on_clear_route_mrm, this)); + mrm_.srv_set_waypoint_route = create_service( + "~/mrm/set_waypoint_route", + service_utils::handle_exception(&RouteSelector::on_set_waypoint_route_mrm, this)); + mrm_.srv_set_lanelet_route = create_service( + "~/mrm/set_lanelet_route", + service_utils::handle_exception(&RouteSelector::on_set_lanelet_route_mrm, this)); + mrm_.pub_state_ = create_publisher("~/mrm/state", durable_qos); + mrm_.pub_route_ = create_publisher("~/mrm/route", durable_qos); + + // Init mission planner interface. + group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cli_clear_route_ = create_client("~/planner/clear_route", service_qos, group_); + cli_set_lanelet_route_ = + create_client("~/planner/set_lanelet_route", service_qos, group_); + cli_set_waypoint_route_ = + create_client("~/planner/set_waypoint_route", service_qos, group_); + sub_state_ = create_subscription( + "~/planner/state", durable_qos, std::bind(&RouteSelector::on_state, this, _1)); + sub_route_ = create_subscription( + "~/planner/route", durable_qos, std::bind(&RouteSelector::on_route, this, _1)); + + // Set initial state. + main_.change_state(RouteState::INITIALIZING); + mrm_.change_state(RouteState::INITIALIZING); + initialized_ = false; + mrm_operating_ = false; + main_request_ = std::monostate{}; +} + +void RouteSelector::on_state(const RouteState::ConstSharedPtr msg) +{ + if (msg->state == RouteState::UNSET && !initialized_) { + main_.change_state(RouteState::UNSET); + mrm_.change_state(RouteState::UNSET); + initialized_ = true; + } + + (mrm_operating_ ? mrm_ : main_).update_state(*msg); +} + +void RouteSelector::on_route(const LaneletRoute::ConstSharedPtr msg) +{ + (mrm_operating_ ? mrm_ : main_).update_route(*msg); +} + +void RouteSelector::on_clear_route_main( + ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res) +{ + // Save the request and clear old route to resume from MRM. + main_request_ = std::monostate{}; + main_.change_route(); + + // During MRM, only change the state. + if (mrm_operating_) { + main_.change_state(RouteState::UNSET); + res->status.success = true; + return; + } + + // Forward the request if not in MRM. + res->status = service_utils::sync_call(cli_clear_route_, req); +} + +void RouteSelector::on_set_waypoint_route_main( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res) +{ + // Save the request and clear old route to resume from MRM. + req->uuid = uuid::generate_if_empty(req->uuid); + main_request_ = req; + main_.change_route(); + + // During MRM, only change the state. + if (mrm_operating_) { + main_.change_state(RouteState::INTERRUPTED); + res->status.success = true; + return; + } + + // Forward the request if not in MRM. + res->status = service_utils::sync_call(cli_set_waypoint_route_, req); +} + +void RouteSelector::on_set_lanelet_route_main( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res) +{ + // Save the request and clear old route to resume from MRM. + req->uuid = uuid::generate_if_empty(req->uuid); + main_request_ = req; + main_.change_route(); + + // During MRM, only change the state. + if (mrm_operating_) { + main_.change_state(RouteState::INTERRUPTED); + res->status.success = true; + return; + } + + // Forward the request if not in MRM. + res->status = service_utils::sync_call(cli_set_lanelet_route_, req); +} + +void RouteSelector::on_clear_route_mrm( + ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res) +{ + res->status = resume_main_route(req); + + if (res->status.success) { + mrm_operating_ = false; + mrm_.change_state(RouteState::UNSET); + } +} + +void RouteSelector::on_set_waypoint_route_mrm( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res) +{ + req->uuid = uuid::generate_if_empty(req->uuid); + res->status = service_utils::sync_call(cli_set_waypoint_route_, req); + + if (res->status.success) { + mrm_operating_ = true; + if (main_.get_state() != RouteState::UNSET) { + main_.change_state(RouteState::INTERRUPTED); + } + } +} + +void RouteSelector::on_set_lanelet_route_mrm( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res) +{ + req->uuid = uuid::generate_if_empty(req->uuid); + res->status = service_utils::sync_call(cli_set_lanelet_route_, req); + + if (res->status.success) { + mrm_operating_ = true; + if (main_.get_state() != RouteState::UNSET) { + main_.change_state(RouteState::INTERRUPTED); + } + } +} + +ResponseStatus RouteSelector::resume_main_route(ClearRoute::Request::SharedPtr req) +{ + const auto create_lanelet_request = [](const LaneletRoute & route) { + // NOTE: The start_pose.is not included in the request. + const auto r = std::make_shared(); + r->header = route.header; + r->goal_pose = route.goal_pose; + r->segments = route.segments; + r->uuid = route.uuid; + r->allow_modification = route.allow_modification; + return r; + }; + + const auto create_goal_request = [](const auto & request) { + const auto r = std::make_shared(); + r->header = request->header; + r->goal_pose = request->goal_pose; + r->uuid = request->uuid; + r->allow_modification = request->allow_modification; + return r; + }; + + // Clear the route if there is no request for the main route. + if (std::holds_alternative(main_request_)) { + return service_utils::sync_call(cli_clear_route_, req); + } + + // Attempt to resume the main route if there is a planned route. + if (const auto route = main_.get_route()) { + const auto r = create_lanelet_request(route.value()); + const auto status = service_utils::sync_call(cli_set_lanelet_route_, r); + if (status.success) return status; + } + + // If resuming fails, replan main route using the goal. + // NOTE: Clear the waypoints to avoid returning. Remove this once resuming is supported. + if (const auto request = std::get_if(&main_request_)) { + const auto r = create_goal_request(*request); + return service_utils::sync_call(cli_set_waypoint_route_, r); + } + if (const auto request = std::get_if(&main_request_)) { + const auto r = create_goal_request(*request); + return service_utils::sync_call(cli_set_waypoint_route_, r); + } + throw std::logic_error("route_selector: unknown main route request"); +} + +} // namespace mission_planner + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mission_planner::RouteSelector) diff --git a/planning/mission_planner/src/mission_planner/route_selector.hpp b/planning/mission_planner/src/mission_planner/route_selector.hpp new file mode 100644 index 0000000000000..3226d1a7cb993 --- /dev/null +++ b/planning/mission_planner/src/mission_planner/route_selector.hpp @@ -0,0 +1,106 @@ +// 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 MISSION_PLANNER__ROUTE_SELECTOR_HPP_ +#define MISSION_PLANNER__ROUTE_SELECTOR_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace mission_planner +{ + +using autoware_common_msgs::msg::ResponseStatus; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_planning_msgs::msg::RouteState; +using tier4_planning_msgs::srv::ClearRoute; +using tier4_planning_msgs::srv::SetLaneletRoute; +using tier4_planning_msgs::srv::SetWaypointRoute; +using unique_identifier_msgs::msg::UUID; + +class RouteInterface +{ +public: + explicit RouteInterface(rclcpp::Clock::SharedPtr clock); + RouteState::_state_type get_state() const; + std::optional get_route() const; + void change_route(); + void change_state(RouteState::_state_type state); + void update_state(const RouteState & state); + void update_route(const LaneletRoute & route); + + rclcpp::Service::SharedPtr srv_clear_route; + rclcpp::Service::SharedPtr srv_set_lanelet_route; + rclcpp::Service::SharedPtr srv_set_waypoint_route; + rclcpp::Publisher::SharedPtr pub_state_; + rclcpp::Publisher::SharedPtr pub_route_; + +private: + RouteState state_; + std::optional route_; + rclcpp::Clock::SharedPtr clock_; +}; + +class RouteSelector : public rclcpp::Node +{ +public: + explicit RouteSelector(const rclcpp::NodeOptions & options); + +private: + using WaypointRequest = SetWaypointRoute::Request::SharedPtr; + using LaneletRequest = SetLaneletRoute::Request::SharedPtr; + + RouteInterface main_; + RouteInterface mrm_; + + rclcpp::CallbackGroup::SharedPtr group_; + rclcpp::Client::SharedPtr cli_clear_route_; + rclcpp::Client::SharedPtr cli_set_waypoint_route_; + rclcpp::Client::SharedPtr cli_set_lanelet_route_; + rclcpp::Subscription::SharedPtr sub_state_; + rclcpp::Subscription::SharedPtr sub_route_; + + bool initialized_; + bool mrm_operating_; + std::variant main_request_; + + void on_state(const RouteState::ConstSharedPtr msg); + void on_route(const LaneletRoute::ConstSharedPtr msg); + + void on_clear_route_main(ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res); + void on_set_waypoint_route_main( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res); + void on_set_lanelet_route_main( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res); + + void on_clear_route_mrm(ClearRoute::Request::SharedPtr req, ClearRoute::Response::SharedPtr res); + void on_set_waypoint_route_mrm( + SetWaypointRoute::Request::SharedPtr req, SetWaypointRoute::Response::SharedPtr res); + void on_set_lanelet_route_mrm( + SetLaneletRoute::Request::SharedPtr req, SetLaneletRoute::Response::SharedPtr res); + + ResponseStatus resume_main_route(ClearRoute::Request::SharedPtr req); +}; + +} // namespace mission_planner + +#endif // MISSION_PLANNER__ROUTE_SELECTOR_HPP_ diff --git a/planning/mission_planner/src/mission_planner/service_utils.cpp b/planning/mission_planner/src/mission_planner/service_utils.cpp new file mode 100644 index 0000000000000..9b3fc77424d5b --- /dev/null +++ b/planning/mission_planner/src/mission_planner/service_utils.cpp @@ -0,0 +1,32 @@ +// 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 "service_utils.hpp" + +#include + +namespace service_utils +{ + +ServiceException ServiceUnready(const std::string & message) +{ + return ServiceException(ResponseStatus::SERVICE_UNREADY, message, false); +} + +ServiceException TransformError(const std::string & message) +{ + return ServiceException(ResponseStatus::TRANSFORM_ERROR, message, false); +}; + +} // namespace service_utils diff --git a/planning/mission_planner/src/mission_planner/service_utils.hpp b/planning/mission_planner/src/mission_planner/service_utils.hpp new file mode 100644 index 0000000000000..6e942ead9b383 --- /dev/null +++ b/planning/mission_planner/src/mission_planner/service_utils.hpp @@ -0,0 +1,80 @@ +// 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 MISSION_PLANNER__SERVICE_UTILS_HPP_ +#define MISSION_PLANNER__SERVICE_UTILS_HPP_ + +#include + +#include +#include + +namespace service_utils +{ + +using ResponseStatus = autoware_common_msgs::msg::ResponseStatus; +using ResponseStatusCode = ResponseStatus::_code_type; + +class ServiceException : public std::exception +{ +public: + ServiceException(ResponseStatusCode code, const std::string & message, bool success = false) + { + success_ = success; + code_ = code; + message_ = message; + } + + template + void set(StatusT & status) const + { + status.success = success_; + status.code = code_; + status.message = message_; + } + +private: + bool success_; + ResponseStatusCode code_; + std::string message_; +}; + +ServiceException ServiceUnready(const std::string & message); +ServiceException TransformError(const std::string & message); + +template +std::function handle_exception(void (T::*callback)(Req, Res), T * instance) +{ + return [instance, callback](Req req, Res res) { + try { + (instance->*callback)(req, res); + } catch (const ServiceException & error) { + error.set(res->status); + } + }; +} + +template +ResponseStatus sync_call(T & client, Req req) +{ + if (!client->service_is_ready()) { + throw ServiceUnready(client->get_service_name()); + } + auto future = client->async_send_request(req); + return future.get()->status; +} + +} // namespace service_utils + +#endif // MISSION_PLANNER__SERVICE_UTILS_HPP_ diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 7f7961f438284..83c814b70f0a5 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -327,6 +327,7 @@ class RouteHandler lanelet::ConstLanelets getShoulderLanelets() const; bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const; bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const; + lanelet::ConstLanelets getPreferredLanelets() const; // for path PathWithLaneId getCenterLinePath( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 90c44d9eaecd0..68be0ac33e934 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -433,6 +433,11 @@ lanelet::ConstLanelets RouteHandler::getRouteLanelets() const return route_lanelets_; } +lanelet::ConstLanelets RouteHandler::getPreferredLanelets() const +{ + return preferred_lanelets_; +} + Pose RouteHandler::getStartPose() const { if (!route_ptr_) { diff --git a/sensing/pointcloud_preprocessor/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index c5f15ec4e49d5..219f43e48730a 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -211,6 +211,7 @@ ament_export_targets(export_${PROJECT_NAME}) ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml new file mode 100644 index 0000000000000..c932e7f62d5bc --- /dev/null +++ b/sensing/pointcloud_preprocessor/config/blockage_diagnostics_param_file.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + blockage_ratio_threshold: 0.1 + blockage_count_threshold: 50 + blockage_buffering_frames: 2 + blockage_buffering_interval: 1 + dust_ratio_threshold: 0.2 + dust_count_threshold: 10 + dust_kernel_size: 2 + dust_buffering_frames: 10 + dust_buffering_interval: 1 + max_distance_range: 200.0 + horizontal_resolution: 0.4 + blockage_kernel: 10 diff --git a/sensing/pointcloud_preprocessor/docs/blockage_diag.md b/sensing/pointcloud_preprocessor/docs/blockage_diag.md index 32e5a5869109d..082ae180fa3c9 100644 --- a/sensing/pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/pointcloud_preprocessor/docs/blockage_diag.md @@ -58,14 +58,17 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref | `horizontal_ring_id` | int | The id of horizontal ring of the LiDAR | | `angle_range` | vector | The effective range of LiDAR | | `vertical_bins` | int | The LiDAR channel number | -| `model` | string | The LiDAR model | +| `is_channel_order_top2down` | bool | If the lidar channels are indexed from top to down | | `blockage_buffering_frames` | int | The number of buffering about blockage detection [range:1-200] | | `blockage_buffering_interval` | int | The interval of buffering about blockage detection | | `dust_ratio_threshold` | float | The threshold of dusty area ratio | | `dust_count_threshold` | int | The threshold of number continuous frames include dusty area | +| `blockage_kernel` | int | The kernel size of morphology processing the detected blockage area | | `dust_kernel_size` | int | The kernel size of morphology processing in dusty area detection | | `dust_buffering_frames` | int | The number of buffering about dusty area detection [range:1-200] | | `dust_buffering_interval` | int | The interval of buffering about dusty area detection | +| `max_distance_range` | double | Maximum view range for the LiDAR | +| `horizontal_resolution` | double | The horizontal resolution of depth map image [deg/pixel] | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 42ca694235a00..9dcc8b1a03715 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -22,13 +22,14 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| Name | Type | Default Value | Description | +| ------------------------- | ------- | ------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | +| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| `publish_excluded_points` | bool | false | Flag to publish excluded pointcloud for debugging purpose. Due to performance concerns, please set to false during experiments. | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp index 66e14c418b52c..c1cf1e59f0343 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/blockage_diag/blockage_diag_nodelet.hpp @@ -33,6 +33,8 @@ #include #endif +#include + #include #include @@ -80,7 +82,7 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int ground_blockage_count_ = 0; int sky_blockage_count_ = 0; int blockage_count_threshold_; - std::string lidar_model_; + bool is_channel_order_top2down_; int blockage_buffering_frames_; int blockage_buffering_interval_; int dust_kernel_size_; @@ -89,6 +91,10 @@ class BlockageDiagComponent : public pointcloud_preprocessor::Filter int dust_buffering_frame_counter_ = 0; int dust_count_threshold_; int dust_frame_count_ = 0; + double max_distance_range_{200.0}; + double horizontal_resolution_{0.4}; + boost::circular_buffer no_return_mask_buffer{1}; + boost::circular_buffer dust_mask_buffer{1}; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index be96aa94382dd..d1560cccd0b15 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -126,6 +126,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node double timeout_sec_ = 0.1; bool publish_synchronized_pointcloud_; + std::string synchronized_pointcloud_postfix_; std::set not_subscribed_topic_names_; @@ -179,6 +180,8 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node void timer_callback(); void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + std::string replaceSyncTopicNamePostfix( + const std::string & original_topic_name, const std::string & postfix); /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index ab7d4e0012304..57ed7a508ca54 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -21,6 +21,7 @@ #include +#include #include #include @@ -42,11 +43,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter const TransformInfo & transform_info); private: + /** \brief publisher of excluded pointcloud for debug reason. **/ + rclcpp::Publisher::SharedPtr excluded_points_publisher_; + double distance_ratio_; double object_length_threshold_; int num_points_threshold_; uint16_t max_rings_num_; size_t max_points_num_per_ring_; + bool publish_excluded_points_; /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -68,6 +73,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter return x * x + y * y + z * z >= object_length_threshold_ * object_length_threshold_; } + PointCloud2 extractExcludedPoints( + const PointCloud2 & input, const PointCloud2 & output, float epsilon); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp index 8c3cb98ba266e..4a36c32980b0e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/time_synchronizer/time_synchronizer_nodelet.hpp @@ -177,6 +177,8 @@ class PointCloudDataSynchronizerComponent : public rclcpp::Node void timer_callback(); void checkSyncStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); + std::string replaceSyncTopicNamePostfix( + const std::string & original_topic_name, const std::string & postfix); /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; diff --git a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml index 8fa5796e2802d..5a93cb950b827 100644 --- a/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml +++ b/sensing/pointcloud_preprocessor/launch/blockage_diag.launch.xml @@ -1,37 +1,18 @@ - - - - - - - - - - - - + + - - - - - - - - - - - + + diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index e1a396c69b3e9..936c52ef81b37 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -18,6 +18,7 @@ ament_cmake_auto autoware_cmake + autoware_auto_geometry autoware_point_types cgal cv_bridge diff --git a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp index 0dd7088f80262..17085ec0a3120 100644 --- a/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/blockage_diag/blockage_diag_nodelet.cpp @@ -16,8 +16,6 @@ #include "autoware_point_types/types.hpp" -#include - #include #include @@ -35,7 +33,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_ratio_threshold_ = declare_parameter("blockage_ratio_threshold"); vertical_bins_ = declare_parameter("vertical_bins"); angle_range_deg_ = declare_parameter>("angle_range"); - lidar_model_ = declare_parameter("model"); + is_channel_order_top2down_ = declare_parameter("is_channel_order_top2down"); blockage_count_threshold_ = declare_parameter("blockage_count_threshold"); blockage_buffering_frames_ = declare_parameter("blockage_buffering_frames"); blockage_buffering_interval_ = declare_parameter("blockage_buffering_interval"); @@ -44,6 +42,17 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options dust_kernel_size_ = declare_parameter("dust_kernel_size"); dust_buffering_frames_ = declare_parameter("dust_buffering_frames"); dust_buffering_interval_ = declare_parameter("dust_buffering_interval"); + max_distance_range_ = declare_parameter("max_distance_range"); + horizontal_resolution_ = declare_parameter("horizontal_resolution"); + blockage_kernel_ = declare_parameter("blockage_kernel"); + } + dust_mask_buffer.set_capacity(dust_buffering_frames_); + no_return_mask_buffer.set_capacity(blockage_buffering_frames_); + if (vertical_bins_ <= horizontal_ring_id_) { + RCLCPP_ERROR( + this->get_logger(), + "The horizontal_ring_id should be smaller than vertical_bins. Skip blockage diag!"); + return; } updater_.setHardwareID("blockage_diag"); @@ -150,24 +159,17 @@ void BlockageDiagComponent::filter( std::scoped_lock lock(mutex_); int vertical_bins = vertical_bins_; int ideal_horizontal_bins; - float distance_coefficient = 327.67f; - float horizontal_resolution_ = 0.4f; - if (lidar_model_ == "Pandar40P") { - distance_coefficient = 327.67f; - horizontal_resolution_ = 0.4f; - } else if (lidar_model_ == "PandarQT") { - distance_coefficient = 3276.75f; - horizontal_resolution_ = 0.6f; + double compensate_angle = 0.0; + // Check the case when angle_range_deg_[1] exceed 360 and shifted the range to 0~360 + if (angle_range_deg_[0] > angle_range_deg_[1]) { + compensate_angle = 360.0; } - ideal_horizontal_bins = - static_cast((angle_range_deg_[1] - angle_range_deg_[0]) / horizontal_resolution_); + ideal_horizontal_bins = static_cast( + (angle_range_deg_[1] + compensate_angle - angle_range_deg_[0]) / horizontal_resolution_); pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); - std::vector horizontal_bin_reference(ideal_horizontal_bins); - std::vector> each_ring_pointcloud(vertical_bins); cv::Mat full_size_depth_map( cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); - cv::Mat lidar_depth_map(cv::Size(ideal_horizontal_bins, vertical_bins), CV_16UC1, cv::Scalar(0)); cv::Mat lidar_depth_map_8u( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); if (pcl_input->points.empty()) { @@ -184,24 +186,23 @@ void BlockageDiagComponent::filter( sky_blockage_range_deg_[0] = angle_range_deg_[0]; sky_blockage_range_deg_[1] = angle_range_deg_[1]; } else { - for (int i = 0; i < ideal_horizontal_bins; ++i) { - horizontal_bin_reference.at(i) = angle_range_deg_[0] + i * horizontal_resolution_; - } for (const auto p : pcl_input->points) { - for (int horizontal_bin = 0; - horizontal_bin < static_cast(horizontal_bin_reference.size()); horizontal_bin++) { - if ( - (p.azimuth / 100 > - (horizontal_bin_reference.at(horizontal_bin) - horizontal_resolution_ / 2)) && - (p.azimuth / 100 <= - (horizontal_bin_reference.at(horizontal_bin) + horizontal_resolution_ / 2))) { - if (lidar_model_ == "Pandar40P") { - full_size_depth_map.at(p.ring, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } else if (lidar_model_ == "PandarQT") { - full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin) = - UINT16_MAX - distance_coefficient * p.distance; - } + double azimuth_deg = p.azimuth / 100.; + if ( + ((azimuth_deg > angle_range_deg_[0]) && + (azimuth_deg <= angle_range_deg_[1] + compensate_angle)) || + ((azimuth_deg + compensate_angle > angle_range_deg_[0]) && + (azimuth_deg < angle_range_deg_[1]))) { + double current_angle_range = (azimuth_deg + compensate_angle - angle_range_deg_[0]); + int horizontal_bin_index = static_cast(current_angle_range / horizontal_resolution_) % + static_cast(360.0 / horizontal_resolution_); + uint16_t depth_intensity = + UINT16_MAX * (1.0 - std::min(p.distance / max_distance_range_, 1.0)); + if (is_channel_order_top2down_) { + full_size_depth_map.at(p.ring, horizontal_bin_index) = depth_intensity; + } else { + full_size_depth_map.at(vertical_bins - p.ring - 1, horizontal_bin_index) = + depth_intensity; } } } @@ -215,7 +216,6 @@ void BlockageDiagComponent::filter( cv::Point(blockage_kernel_, blockage_kernel_)); cv::erode(no_return_mask, erosion_dst, blockage_element); cv::dilate(erosion_dst, no_return_mask, blockage_element); - static boost::circular_buffer no_return_mask_buffer(blockage_buffering_frames_); cv::Mat time_series_blockage_result( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); cv::Mat time_series_blockage_mask( @@ -250,8 +250,13 @@ void BlockageDiagComponent::filter( ground_blockage_ratio_ = static_cast(cv::countNonZero(ground_no_return_mask)) / static_cast(ideal_horizontal_bins * (vertical_bins - horizontal_ring_id_)); - sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / - static_cast(ideal_horizontal_bins * horizontal_ring_id_); + + if (horizontal_ring_id_ == 0) { + sky_blockage_ratio_ = 0.0f; + } else { + sky_blockage_ratio_ = static_cast(cv::countNonZero(sky_no_return_mask)) / + static_cast(ideal_horizontal_bins * horizontal_ring_id_); + } if (ground_blockage_ratio_ > blockage_ratio_threshold_) { cv::Rect ground_blockage_bb = cv::boundingRect(ground_no_return_mask); @@ -295,7 +300,6 @@ void BlockageDiagComponent::filter( cv::inRange(single_dust_ground_img, 254, 255, single_dust_ground_img); cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); - static boost::circular_buffer dust_mask_buffer(dust_buffering_frames_); cv::Mat binarized_dust_mask_( cv::Size(ideal_horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); cv::Mat multi_frame_dust_mask( @@ -405,8 +409,8 @@ rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback( RCLCPP_DEBUG( get_logger(), "Setting new blockage_count_threshold to: %d.", blockage_count_threshold_); } - if (get_param(p, "model", lidar_model_)) { - RCLCPP_DEBUG(get_logger(), "Setting new lidar model to: %s. ", lidar_model_.c_str()); + if (get_param(p, "is_channel_order_top2down", is_channel_order_top2down_)) { + RCLCPP_DEBUG(get_logger(), "Setting new lidar model to: %d. ", is_channel_order_top2down_); } if (get_param(p, "angle_range", angle_range_deg_)) { RCLCPP_DEBUG( diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index f0b04bacf26dd..f6407c532b5f8 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -61,7 +61,8 @@ #include #include -#define POSTFIX_NAME "_synchronized" +#define DEFAULT_SYNC_TOPIC_POSTFIX \ + "_synchronized" // default postfix name for synchronized pointcloud ////////////////////////////////////////////////////////////////////////////////////////////// @@ -112,6 +113,8 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro // Check if publish synchronized pointcloud publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", false); + synchronized_pointcloud_postfix_ = + declare_parameter("synchronized_pointcloud_postfix", "pointcloud"); } // Initialize not_subscribed_topic_names_ @@ -185,10 +188,10 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro } } - // Transformed Raw PointCloud2 Publisher - { + // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud + if (publish_synchronized_pointcloud_) { for (auto & topic : input_topics_) { - std::string new_topic = topic + POSTFIX_NAME; + std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix_); auto publisher = this->create_publisher( new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); transformed_raw_pc_publisher_map_.insert({topic, publisher}); @@ -231,6 +234,35 @@ void PointCloudConcatenateDataSynchronizerComponent::transformPointCloud( } } +std::string PointCloudConcatenateDataSynchronizerComponent::replaceSyncTopicNamePostfix( + const std::string & original_topic_name, const std::string & postfix) +{ + std::string replaced_topic_name; + // separate the topic name by '/' and replace the last element with the new postfix + size_t pos = original_topic_name.find_last_of("/"); + if (pos == std::string::npos) { + // not found '/': this is not a namespaced topic + RCLCPP_WARN_STREAM( + get_logger(), + "The topic name is not namespaced. The postfix will be added to the end of the topic name."); + return original_topic_name + postfix; + } else { + // replace the last element with the new postfix + replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix; + } + + // if topic name is the same with original topic name, add postfix to the end of the topic name + if (replaced_topic_name == original_topic_name) { + RCLCPP_WARN_STREAM( + get_logger(), "The topic name " + << original_topic_name + << " have the same postfix with synchronized pointcloud. We use the postfix " + "to the end of the topic name."); + replaced_topic_name = original_topic_name + DEFAULT_SYNC_TOPIC_POSTFIX; + } + return replaced_topic_name; +} + /** * @brief compute transform to adjust for old timestamp * diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index 4e26adb19bda5..d0a4fc892e940 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -74,12 +74,6 @@ PointCloudConcatenationComponent::PointCloudConcatenationComponent( return; } } - // add postfix to topic names - { - for (auto & topic : input_topics_) { - topic = topic + POSTFIX_NAME; - } - } // Initialize not_subscribed_topic_names_ { diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index d968b06a0dc61..e0c8443cfe57b 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,8 +14,12 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "autoware_auto_geometry/common_3d.hpp" + #include +#include + #include #include namespace pointcloud_preprocessor @@ -29,6 +33,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions using tier4_autoware_utils::StopWatch; stop_watch_ptr_ = std::make_unique>(); debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); + excluded_points_publisher_ = + this->create_publisher("debug/ring_outlier_filter", 1); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -42,6 +48,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); max_points_num_per_ring_ = static_cast(declare_parameter("max_points_num_per_ring", 4000)); + publish_excluded_points_ = + static_cast(declare_parameter("publish_excluded_points", false)); } using std::placeholders::_1; @@ -196,6 +204,17 @@ void RingOutlierFilterComponent::faster_filter( sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + if (publish_excluded_points_) { + auto excluded_points = extractExcludedPoints(*input, output, 0.01); + // set fields + sensor_msgs::PointCloud2Modifier excluded_pcd_modifier(excluded_points); + excluded_pcd_modifier.setPointCloud2Fields( + num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + excluded_points_publisher_->publish(excluded_points); + } + // add processing time for debug if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); @@ -241,6 +260,10 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba if (get_param(p, "num_points_threshold", num_points_threshold_)) { RCLCPP_DEBUG(get_logger(), "Setting new num_points_threshold to: %d.", num_points_threshold_); } + if (get_param(p, "publish_excluded_points", publish_excluded_points_)) { + RCLCPP_DEBUG( + get_logger(), "Setting new publish_excluded_points to: %d.", publish_excluded_points_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -248,6 +271,54 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba return result; } + +sensor_msgs::msg::PointCloud2 RingOutlierFilterComponent::extractExcludedPoints( + const sensor_msgs::msg::PointCloud2 & input, const sensor_msgs::msg::PointCloud2 & output, + float epsilon) +{ + // Convert ROS PointCloud2 message to PCL point cloud for easier manipulation + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr output_cloud(new pcl::PointCloud); + pcl::fromROSMsg(input, *input_cloud); + pcl::fromROSMsg(output, *output_cloud); + + pcl::PointCloud::Ptr excluded_points(new pcl::PointCloud); + + pcl::search::Search::Ptr tree; + if (output_cloud->isOrganized()) { + tree.reset(new pcl::search::OrganizedNeighbor()); + } else { + tree.reset(new pcl::search::KdTree(false)); + } + tree->setInputCloud(output_cloud); + std::vector nn_indices(1); + std::vector nn_distances(1); + for (const auto & point : input_cloud->points) { + if (!tree->nearestKSearch(point, 1, nn_indices, nn_distances)) { + continue; + } + if (nn_distances[0] > epsilon) { + excluded_points->points.push_back(point); + } + } + + sensor_msgs::msg::PointCloud2 excluded_points_msg; + pcl::toROSMsg(*excluded_points, excluded_points_msg); + + // Set the metadata for the excluded points message based on the input cloud + excluded_points_msg.height = 1; + excluded_points_msg.width = + static_cast(output.data.size() / output.height / output.point_step); + excluded_points_msg.row_step = static_cast(output.data.size() / output.height); + excluded_points_msg.is_bigendian = input.is_bigendian; + excluded_points_msg.is_dense = input.is_dense; + excluded_points_msg.header = input.header; + excluded_points_msg.header.frame_id = + !tf_input_frame_.empty() ? tf_input_frame_ : tf_input_orig_frame_; + + return excluded_points_msg; +} + } // namespace pointcloud_preprocessor #include diff --git a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp index 8796477f3d9ae..8f02721a67cfb 100644 --- a/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_nodelet.cpp @@ -34,7 +34,7 @@ #include // postfix for output topics -#define POSTFIX_NAME "_synchronized" +#define DEFAULT_SYNC_TOPIC_POSTFIX "_synchronized" ////////////////////////////////////////////////////////////////////////////////////////////// namespace pointcloud_preprocessor @@ -55,6 +55,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( } // Set parameters + std::string synchronized_pointcloud_postfix; { output_frame_ = static_cast(declare_parameter("output_frame", "")); if (output_frame_.empty()) { @@ -71,6 +72,9 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); return; } + // output topic name postfix + synchronized_pointcloud_postfix = + declare_parameter("synchronized_pointcloud_postfix", "pointcloud"); // Optional parameters maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); @@ -150,7 +154,7 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( // Transformed Raw PointCloud2 Publisher { for (auto & topic : input_topics_) { - std::string new_topic = topic + POSTFIX_NAME; + std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix); auto publisher = this->create_publisher( new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_)); transformed_raw_pc_publisher_map_.insert({topic, publisher}); @@ -173,6 +177,35 @@ PointCloudDataSynchronizerComponent::PointCloudDataSynchronizerComponent( } } +std::string PointCloudDataSynchronizerComponent::replaceSyncTopicNamePostfix( + const std::string & original_topic_name, const std::string & postfix) +{ + std::string replaced_topic_name; + // separate the topic name by '/' and replace the last element with the new postfix + size_t pos = original_topic_name.find_last_of("/"); + if (pos == std::string::npos) { + // not found '/': this is not a namespaced topic + RCLCPP_WARN_STREAM( + get_logger(), + "The topic name is not namespaced. The postfix will be added to the end of the topic name."); + return original_topic_name + postfix; + } else { + // replace the last element with the new postfix + replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix; + } + + // if topic name is the same with original topic name, add postfix to the end of the topic name + if (replaced_topic_name == original_topic_name) { + RCLCPP_WARN_STREAM( + get_logger(), "The topic name " + << original_topic_name + << " have the same postfix with synchronized pointcloud. We use the postfix " + "to the end of the topic name."); + replaced_topic_name = original_topic_name + DEFAULT_SYNC_TOPIC_POSTFIX; + } + return replaced_topic_name; +} + ////////////////////////////////////////////////////////////////////////////////////////////// // overloaded functions void PointCloudDataSynchronizerComponent::transformPointCloud( diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp index 44b2f0dfe6ef0..1fc1249b59e50 100644 --- a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid.hpp @@ -111,8 +111,8 @@ class VoxelGridNearestCentroid : public VoxelGrid struct Leaf { /** \brief Constructor. - * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref evals_ to 0 and \ref cov_ and \ref - * evecs_ to the identity matrix + * Sets \ref nr_points, \ref icov_, \ref mean_ and \ref eigen_values_ to 0 and \ref cov_ and + * \ref eigen_vectors_ to the identity matrix */ Leaf() : nr_points(0), @@ -120,8 +120,8 @@ class VoxelGridNearestCentroid : public VoxelGrid centroid() // cov_ (Eigen::Matrix3d::Identity ()), // icov_ (Eigen::Matrix3d::Zero ()), - // evecs_ (Eigen::Matrix3d::Identity ()), - // evals_ (Eigen::Vector3d::Zero ()) + // eigen_vectors_ (Eigen::Matrix3d::Identity ()), + // eigen_values_ (Eigen::Vector3d::Zero ()) { } @@ -153,23 +153,23 @@ class VoxelGridNearestCentroid : public VoxelGrid // } /** \brief Get the eigen vectors of the voxel covariance. - * \note Order corresponds with \ref getEvals + * \note Order corresponds with \ref getEigenValues * \return matrix whose columns contain eigen vectors */ // Eigen::Matrix3d - // getEvecs () const + // getEigenVectors () const // { - // return (evecs_); + // return (eigen_vectors_); // } /** \brief Get the eigen values of the voxel covariance. - * \note Order corresponds with \ref getEvecs + * \note Order corresponds with \ref getEigenVectors * \return vector of eigen values */ // Eigen::Vector3d - // getEvals () const + // getEigenValues () const // { - // return (evals_); + // return (eigen_values_); // } /** \brief Get the number of points contained by this voxel. @@ -195,10 +195,10 @@ class VoxelGridNearestCentroid : public VoxelGrid // Eigen::Matrix3d icov_; /** \brief Eigen vectors of voxel covariance matrix */ - // Eigen::Matrix3d evecs_; + // Eigen::Matrix3d eigen_vectors_; /** \brief Eigen values of voxel covariance matrix */ - // Eigen::Vector3d evals_; + // Eigen::Vector3d eigen_values_; PointCloud points; }; @@ -217,7 +217,7 @@ class VoxelGridNearestCentroid : public VoxelGrid : searchable_(true), // min_points_per_voxel_ (6), min_points_per_voxel_(1), - // min_covar_eigenvalue_mult_ (0.01), + // min_cov_eigenvalue_mult_ (0.01), leaves_(), voxel_centroids_(), voxel_centroids_leaf_indices_(), @@ -258,12 +258,12 @@ class VoxelGridNearestCentroid : public VoxelGrid inline int getMinPointPerVoxel() { return min_points_per_voxel_; } /** \brief Set the minimum allowable ratio between eigenvalues to prevent singular covariance - * matrices. \param[in] min_covar_eigenvalue_mult the minimum allowable ratio between eigenvalues + * matrices. \param[in] min_cov_eigenvalue_mult the minimum allowable ratio between eigenvalues */ // inline void - // setCovEigValueInflationRatio (double min_covar_eigenvalue_mult) + // setCovEigValueInflationRatio (double min_cov_eigenvalue_mult) // { - // min_covar_eigenvalue_mult_ = min_covar_eigenvalue_mult; + // min_cov_eigenvalue_mult_ = min_cov_eigenvalue_mult; // } /** \brief Get the minimum allowable ratio between eigenvalues to prevent singular covariance @@ -272,7 +272,7 @@ class VoxelGridNearestCentroid : public VoxelGrid // inline double // getCovEigValueInflationRatio () // { - // return min_covar_eigenvalue_mult_; + // return min_cov_eigenvalue_mult_; // } /** \brief Filter cloud and initializes voxel structure. @@ -517,7 +517,7 @@ class VoxelGridNearestCentroid : public VoxelGrid /** \brief Minimum allowable ratio between eigenvalues to prevent singular covariance * matrices. */ - // double min_covar_eigenvalue_mult_; + // double min_cov_eigenvalue_mult_; /** \brief Voxel structure containing all leaf nodes (includes voxels with less than * a sufficient number of points). */ diff --git a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp index 6298c4a2e4153..a42b43b448080 100644 --- a/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp +++ b/sensing/tier4_pcl_extensions/include/tier4_pcl_extensions/voxel_grid_nearest_centroid_impl.hpp @@ -300,7 +300,7 @@ void pcl::VoxelGridNearestCentroid::applyFilter(PointCloud & output) // Eigen values less than a threshold of max eigen value are inflated to a set fraction of the // max eigen value. - // double min_covar_eigenvalue; + // double min_cov_eigenvalue; for (typename std::map::iterator it = leaves_.begin(); it != leaves_.end(); ++it) { // Normalize the centroid diff --git a/sensing/tier4_pcl_extensions/package.xml b/sensing/tier4_pcl_extensions/package.xml index 22b053bd7c788..82dd77d4308ca 100644 --- a/sensing/tier4_pcl_extensions/package.xml +++ b/sensing/tier4_pcl_extensions/package.xml @@ -5,6 +5,9 @@ 0.1.0 The tier4_pcl_extensions package Ryu Yamamoto + Kenzo Lobos Tsunekawa + David Wong + Apache License 2.0 ament_cmake_auto diff --git a/system/default_ad_api/src/interface.cpp b/system/default_ad_api/src/interface.cpp index 65d16010951a4..0b90e1c4c79a7 100644 --- a/system/default_ad_api/src/interface.cpp +++ b/system/default_ad_api/src/interface.cpp @@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf { const auto on_interface_version = [](auto, auto res) { res->major = 1; - res->minor = 0; + res->minor = 1; res->patch = 0; }; diff --git a/system/default_ad_api/src/routing.cpp b/system/default_ad_api/src/routing.cpp index 5204146201c8a..0f2247c3aada8 100644 --- a/system/default_ad_api/src/routing.cpp +++ b/system/default_ad_api/src/routing.cpp @@ -18,6 +18,33 @@ #include +namespace +{ + +using autoware_adapi_v1_msgs::msg::ResponseStatus; + +template +ResponseStatus route_already_set() +{ + ResponseStatus status; + status.success = false; + status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE; + status.message = "The route is already set."; + return status; +} + +template +ResponseStatus route_is_not_set() +{ + ResponseStatus status; + status.success = false; + status.code = InterfaceT::Service::Response::ERROR_INVALID_STATE; + status.message = "The route is not set yet."; + return status; +} + +} // namespace + namespace default_ad_api { @@ -30,20 +57,25 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_sub(sub_state_, this, &RoutingNode::on_state); adaptor.init_sub(sub_route_, this, &RoutingNode::on_route); adaptor.init_cli(cli_clear_route_, group_cli_); + adaptor.init_cli(cli_set_waypoint_route_, group_cli_); + adaptor.init_cli(cli_set_lanelet_route_, group_cli_); adaptor.init_srv(srv_clear_route_, this, &RoutingNode::on_clear_route); - adaptor.relay_service(cli_set_route_, srv_set_route_, group_cli_); - adaptor.relay_service(cli_set_route_points_, srv_set_route_points_, group_cli_); - adaptor.relay_service(cli_change_route_, srv_change_route_, group_cli_); - adaptor.relay_service(cli_change_route_points_, srv_change_route_points_, group_cli_); + adaptor.init_srv(srv_set_route_, this, &RoutingNode::on_set_route); + adaptor.init_srv(srv_set_route_points_, this, &RoutingNode::on_set_route_points); + adaptor.init_srv(srv_change_route_, this, &RoutingNode::on_change_route); + adaptor.init_srv(srv_change_route_points_, this, &RoutingNode::on_change_route_points); adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); + + is_auto_mode_ = false; + state_.state = State::Message::UNKNOWN; } void RoutingNode::change_stop_mode() { using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; - if (is_auto_mode) { + if (is_auto_mode_) { const auto req = std::make_shared(); req->mode = OperationModeRequest::STOP; cli_operation_mode_->async_send_request(req); @@ -52,12 +84,19 @@ void RoutingNode::change_stop_mode() void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) { - is_auto_mode = msg->mode == OperationModeState::Message::AUTONOMOUS; + is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS; } void RoutingNode::on_state(const State::Message::ConstSharedPtr msg) { - pub_state_->publish(*msg); + // TODO(Takagi, Isamu): Add adapi initializing state. + // Represent initializing state by not publishing the topic for now. + if (msg->state == State::Message::INITIALIZING) { + return; + } + + state_ = *msg; + pub_state_->publish(conversion::convert_state(*msg)); // Change operation mode to stop when the vehicle arrives. if (msg->state == State::Message::ARRIVED) { @@ -80,7 +119,51 @@ void RoutingNode::on_clear_route( const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res) { change_stop_mode(); - *res = *cli_clear_route_->call(req); + res->status = conversion::convert_call(cli_clear_route_, req); +} + +void RoutingNode::on_set_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::UNSET) { + res->status = route_already_set(); + return; + } + res->status = conversion::convert_call(cli_set_waypoint_route_, req); +} + +void RoutingNode::on_set_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::UNSET) { + res->status = route_already_set(); + return; + } + res->status = conversion::convert_call(cli_set_lanelet_route_, req); +} + +void RoutingNode::on_change_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::SET) { + res->status = route_is_not_set(); + return; + } + res->status = conversion::convert_call(cli_set_waypoint_route_, req); +} + +void RoutingNode::on_change_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res) +{ + if (state_.state != State::Message::SET) { + res->status = route_is_not_set(); + return; + } + res->status = conversion::convert_call(cli_set_lanelet_route_, req); } } // namespace default_ad_api diff --git a/system/default_ad_api/src/routing.hpp b/system/default_ad_api/src/routing.hpp index 7c15f6d64608f..406569aa0a36b 100644 --- a/system/default_ad_api/src/routing.hpp +++ b/system/default_ad_api/src/routing.hpp @@ -33,6 +33,10 @@ class RoutingNode : public rclcpp::Node explicit RoutingNode(const rclcpp::NodeOptions & options); private: + using OperationModeState = system_interface::OperationModeState; + using State = planning_interface::RouteState; + using Route = planning_interface::LaneletRoute; + rclcpp::CallbackGroup::SharedPtr group_cli_; Pub pub_state_; Pub pub_route_; @@ -42,19 +46,15 @@ class RoutingNode : public rclcpp::Node Srv srv_change_route_; Srv srv_clear_route_; Sub sub_state_; - Sub sub_route_; - Cli cli_set_route_points_; - Cli cli_set_route_; - Cli cli_change_route_points_; - Cli cli_change_route_; + Sub sub_route_; + Cli cli_set_waypoint_route_; + Cli cli_set_lanelet_route_; Cli cli_clear_route_; Cli cli_operation_mode_; Sub sub_operation_mode_; - bool is_auto_mode = false; + bool is_auto_mode_; + State::Message state_; - using OperationModeState = system_interface::OperationModeState; - using State = planning_interface::RouteState; - using Route = planning_interface::Route; void change_stop_mode(); void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg); void on_state(const State::Message::ConstSharedPtr msg); @@ -62,6 +62,18 @@ class RoutingNode : public rclcpp::Node void on_clear_route( const autoware_ad_api::routing::ClearRoute::Service::Request::SharedPtr req, const autoware_ad_api::routing::ClearRoute::Service::Response::SharedPtr res); + void on_set_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res); + void on_set_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res); + void on_change_route_points( + const autoware_ad_api::routing::SetRoutePoints::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoutePoints::Service::Response::SharedPtr res); + void on_change_route( + const autoware_ad_api::routing::SetRoute::Service::Request::SharedPtr req, + const autoware_ad_api::routing::SetRoute::Service::Response::SharedPtr res); }; } // namespace default_ad_api diff --git a/system/default_ad_api/src/utils/route_conversion.cpp b/system/default_ad_api/src/utils/route_conversion.cpp index 88226c1fed063..70951fc337b68 100644 --- a/system/default_ad_api/src/utils/route_conversion.cpp +++ b/system/default_ad_api/src/utils/route_conversion.cpp @@ -14,6 +14,7 @@ #include "route_conversion.hpp" +#include #include namespace @@ -47,6 +48,15 @@ RoutePrimitive convert(const LaneletPrimitive & in) return api; } +template <> +LaneletPrimitive convert(const RoutePrimitive & in) +{ + LaneletPrimitive out; + out.id = in.id; + out.primitive_type = in.type; + return out; +} + template <> RouteSegment convert(const LaneletSegment & in) { @@ -62,6 +72,18 @@ RouteSegment convert(const LaneletSegment & in) return api; } +template <> +LaneletSegment convert(const RouteSegment & in) +{ + LaneletSegment out; + out.preferred_primitive = convert(in.preferred); + out.primitives.push_back(out.preferred_primitive); + for (const auto & primitive : in.alternatives) { + out.primitives.push_back(convert(primitive)); + } + return out; +} + } // namespace namespace default_ad_api::conversion @@ -87,4 +109,65 @@ ExternalRoute convert_route(const InternalRoute & internal) return external; } +ExternalState convert_state(const InternalState & internal) +{ + // clang-format off + const auto convert = [](InternalState::_state_type state) { + switch(state) { + // TODO(Takagi, Isamu): Add adapi state. + case InternalState::INITIALIZING: return ExternalState::UNSET; + case InternalState::UNSET: return ExternalState::UNSET; + case InternalState::ROUTING: return ExternalState::UNSET; + case InternalState::SET: return ExternalState::SET; + case InternalState::REROUTING: return ExternalState::CHANGING; + case InternalState::ARRIVED: return ExternalState::ARRIVED; + case InternalState::ABORTED: return ExternalState::SET; + case InternalState::INTERRUPTED: return ExternalState::SET; + default: return ExternalState::UNKNOWN; + } + }; + // clang-format on + + ExternalState external; + external.stamp = internal.stamp; + external.state = convert(internal.state); + return external; +} + +InternalClearRequest convert_request(const ExternalClearRequest &) +{ + auto internal = std::make_shared(); + return internal; +} + +InternalLaneletRequest convert_request(const ExternalLaneletRequest & external) +{ + auto internal = std::make_shared(); + internal->header = external->header; + internal->goal_pose = external->goal; + internal->segments = convert_vector(external->segments); + internal->allow_modification = external->option.allow_goal_modification; + return internal; +} + +InternalWaypointRequest convert_request(const ExternalWaypointRequest & external) +{ + auto internal = std::make_shared(); + internal->header = external->header; + internal->goal_pose = external->goal; + internal->waypoints = external->waypoints; + internal->allow_modification = external->option.allow_goal_modification; + return internal; +} + +ExternalResponse convert_response(const InternalResponse & internal) +{ + // TODO(Takagi, Isamu): check error code correspondence + ExternalResponse external; + external.success = internal.success; + external.code = internal.code; + external.message = internal.message; + return external; +} + } // namespace default_ad_api::conversion diff --git a/system/default_ad_api/src/utils/route_conversion.hpp b/system/default_ad_api/src/utils/route_conversion.hpp index 19f4320228fa8..9a31b9e80c064 100644 --- a/system/default_ad_api/src/utils/route_conversion.hpp +++ b/system/default_ad_api/src/utils/route_conversion.hpp @@ -18,18 +18,50 @@ #include #include +#include +#include #include +#include #include +#include +#include +#include +#include namespace default_ad_api::conversion { using ExternalRoute = autoware_adapi_v1_msgs::msg::Route; using InternalRoute = autoware_planning_msgs::msg::LaneletRoute; - ExternalRoute create_empty_route(const rclcpp::Time & stamp); ExternalRoute convert_route(const InternalRoute & internal); +using ExternalState = autoware_adapi_v1_msgs::msg::RouteState; +using InternalState = tier4_planning_msgs::msg::RouteState; +ExternalState convert_state(const InternalState & internal); + +using ExternalClearRequest = autoware_adapi_v1_msgs::srv::ClearRoute::Request::SharedPtr; +using InternalClearRequest = tier4_planning_msgs::srv::ClearRoute::Request::SharedPtr; +InternalClearRequest convert_request(const ExternalClearRequest & external); + +using ExternalLaneletRequest = autoware_adapi_v1_msgs::srv::SetRoute::Request::SharedPtr; +using InternalLaneletRequest = tier4_planning_msgs::srv::SetLaneletRoute::Request::SharedPtr; +InternalLaneletRequest convert_request(const ExternalLaneletRequest & external); + +using ExternalWaypointRequest = autoware_adapi_v1_msgs::srv::SetRoutePoints::Request::SharedPtr; +using InternalWaypointRequest = tier4_planning_msgs::srv::SetWaypointRoute::Request::SharedPtr; +InternalWaypointRequest convert_request(const ExternalWaypointRequest & external); + +using ExternalResponse = autoware_adapi_v1_msgs::msg::ResponseStatus; +using InternalResponse = autoware_common_msgs::msg::ResponseStatus; +ExternalResponse convert_response(const InternalResponse & internal); + +template +ExternalResponse convert_call(ClientT & client, RequestT & req) +{ + return convert_response(client->call(convert_request(req))->status); +} + } // namespace default_ad_api::conversion #endif // UTILS__ROUTE_CONVERSION_HPP_ diff --git a/system/default_ad_api/test/node/interface_version.py b/system/default_ad_api/test/node/interface_version.py index 19be3dc714c22..4694f3af642ce 100644 --- a/system/default_ad_api/test/node/interface_version.py +++ b/system/default_ad_api/test/node/interface_version.py @@ -30,7 +30,7 @@ if response.major != 1: exit(1) -if response.minor != 0: +if response.minor != 1: exit(1) if response.patch != 0: exit(1) diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml index 54b4f691b6eb1..96dce7eb360e4 100644 --- a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -1,4 +1,4 @@ /**: ros__parameters: update_rate: 10.0 - add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg + add_duplicated_node_names_to_msg: true # if true, duplicated node names are added to msg diff --git a/system/duplicated_node_checker/package.xml b/system/duplicated_node_checker/package.xml index e2ba225b16330..e25758ba51557 100644 --- a/system/duplicated_node_checker/package.xml +++ b/system/duplicated_node_checker/package.xml @@ -6,6 +6,7 @@ A package to find out nodes with common names Shumpei Wakabayashi yliuhb + Mamoru Sobue Apache 2.0 ament_cmake diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index 46c02d9d6e133..384c8fdc46c5f 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -69,7 +69,7 @@ void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticSta if (add_duplicated_node_names_to_msg_) { std::set unique_identical_names(identical_names.begin(), identical_names.end()); for (const auto & name : unique_identical_names) { - msg += " " + name; + msg += "[" + name + "], "; } } for (auto name : identical_names) { diff --git a/system/mrm_handler/CMakeLists.txt b/system/mrm_handler/CMakeLists.txt new file mode 100644 index 0000000000000..7c2174f03a6f3 --- /dev/null +++ b/system/mrm_handler/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(mrm_handler) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(mrm_handler + src/mrm_handler/mrm_handler_node.cpp + src/mrm_handler/mrm_handler_core.cpp +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md new file mode 100644 index 0000000000000..4944d8a833643 --- /dev/null +++ b/system/mrm_handler/README.md @@ -0,0 +1,44 @@ +# mrm_handler + +## Purpose + +MRM Handler is a node to select a proper MRM from a system failure state contained in OperationModeAvailability. + +## Inner-workings / Algorithms + +### State Transitions + +![mrm-state](image/mrm-state.svg) + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | +| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | + +### Output + +| Name | Type | Description | +| --------------------------------------- | ------------------------------------------------------ | ----------------------------------------------------- | +| `/system/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | + +## Parameters + +{{ json_to_markdown("system/mrm_handler/schema/mrm_handler.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/mrm_handler/config/mrm_handler.param.yaml new file mode 100644 index 0000000000000..292d459f69d88 --- /dev/null +++ b/system/mrm_handler/config/mrm_handler.param.yaml @@ -0,0 +1,17 @@ +# Default configuration for mrm handler +--- +/**: + ros__parameters: + update_rate: 10 + timeout_operation_mode_availability: 0.5 + use_emergency_holding: false + timeout_emergency_recovery: 5.0 + timeout_takeover_request: 10.0 + use_takeover_request: false + use_parking_after_stopped: false + use_pull_over: false + use_comfortable_stop: false + + # setting whether to turn hazard lamp on for each situation + turning_hazard_on: + emergency: true diff --git a/system/mrm_handler/image/mrm-state.svg b/system/mrm_handler/image/mrm-state.svg new file mode 100644 index 0000000000000..13a2956b6f15c --- /dev/null +++ b/system/mrm_handler/image/mrm-state.svg @@ -0,0 +1,362 @@ + + + + + + + + + + + +
+
+
+ not emergency +
+
+
+ + not emergency + + + + + + + + + + + +
+
+
+ emergency +
+
+
+ + emergency + + + + + + +
+
+
+ NORMAL +
+
+
+ + NORMAL + + + + + + + +
+
+
+ succeeded +
+
+
+ + succeeded + + + + + + + +
+
+
+ failed +
+
+
+ + failed + + + + + + + + +
+
+
+ MRM_OPERATING +
+
+
+ + MRM_OPERATING + + + + + + + +
+
+
+ pull over unavailable +
+ comfortable_stop available (ca) +
+
+
+ + pull over unavailable... + + + + + + + +
+
+
+ pull over available (pa) +
+
+
+ + pull over available (pa) + + + + + + + +
+
+
+ pull over unavailable +
+ comfortable_stop unavailable +
+ emergency_stop available (ea) +
+
+
+ + pull over unavailable... + + + + + + + + + + + +
+
+
+ pull_over +
+
+
+ + pull_over + + + + + + + +
+
+
+ ea +
+
+
+ + ea + + + + + +
+
+
+ ea +
+
+
+ + ea + + + + + + +
+
+
+ emergency_stop +
+
+
+ + emergency_stop + + + + + + + +
+
+
+ ca +
+
+
+ + ca + + + + + + +
+
+
+ comfortable_stop +
+
+
+ + comfortable_stop + + + + + + + + + +
+
+
+ pa +
+
+
+ + pa + + + + + + +
+
+
+ MRM_SUCCEEDED +
+
+
+ + MRM_SUCCEEDED + + + + + + +
+
+
+ MRM_FAILED +
+
+
+ + MRM_FAILED + + + + + + + Text is not SVG - cannot display + + + diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp new file mode 100644 index 0000000000000..8a6ec81cbafff --- /dev/null +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -0,0 +1,160 @@ +// 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. + +#ifndef MRM_HANDLER__MRM_HANDLER_CORE_HPP_ +#define MRM_HANDLER__MRM_HANDLER_CORE_HPP_ + +// Core +#include +#include +#include +#include + +// Autoware +#include +#include +#include +#include +#include +#include +#include +#include + +// ROS 2 core +#include +#include + +#include +#include + +struct HazardLampPolicy +{ + bool emergency; +}; + +struct Param +{ + int update_rate; + double timeout_operation_mode_availability; + bool use_emergency_holding; + double timeout_emergency_recovery; + double timeout_takeover_request; + bool use_takeover_request; + bool use_parking_after_stopped; + bool use_pull_over; + bool use_comfortable_stop; + HazardLampPolicy turning_hazard_on{}; +}; + +class MrmHandler : public rclcpp::Node +{ +public: + MrmHandler(); + +private: + // Subscribers + rclcpp::Subscription::SharedPtr sub_odom_; + rclcpp::Subscription::SharedPtr + sub_control_mode_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_availability_; + rclcpp::Subscription::SharedPtr + sub_mrm_pull_over_status_; + rclcpp::Subscription::SharedPtr + sub_mrm_comfortable_stop_status_; + rclcpp::Subscription::SharedPtr + sub_mrm_emergency_stop_status_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_state_; + + nav_msgs::msg::Odometry::ConstSharedPtr odom_; + autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_; + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; + tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; + autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; + + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onOperationModeAvailability( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); + void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); + void onMrmComfortableStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); + void onMrmEmergencyStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); + void onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); + + // Publisher + + // rclcpp::Publisher::SharedPtr pub_shift_; + // rclcpp::Publisher::SharedPtr pub_turn_signal_; + rclcpp::Publisher::SharedPtr + pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; + + autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); + autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); + void publishControlCommands(); + + rclcpp::Publisher::SharedPtr pub_mrm_state_; + + autoware_adapi_v1_msgs::msg::MrmState mrm_state_; + void publishMrmState(); + + // Clients + rclcpp::CallbackGroup::SharedPtr client_mrm_pull_over_group_; + rclcpp::Client::SharedPtr client_mrm_pull_over_; + rclcpp::CallbackGroup::SharedPtr client_mrm_comfortable_stop_group_; + rclcpp::Client::SharedPtr client_mrm_comfortable_stop_; + rclcpp::CallbackGroup::SharedPtr client_mrm_emergency_stop_group_; + rclcpp::Client::SharedPtr client_mrm_emergency_stop_; + + void callMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + void cancelMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const; + void logMrmCallingResult( + const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, + bool is_call) const; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Parameters + Param param_; + + bool isDataReady(); + void onTimer(); + + // Heartbeat + rclcpp::Time stamp_operation_mode_availability_; + std::optional stamp_autonomous_become_unavailable_ = std::nullopt; + + // Algorithm + rclcpp::Time takeover_requested_time_; + bool is_takeover_request_ = false; + bool is_emergency_holding_ = false; + void transitionTo(const int new_state); + void updateMrmState(); + void operateMrm(); + autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); + bool isStopped(); + bool isEmergency() const; + bool isArrivedAtGoal(); +}; + +#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_ diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml new file mode 100644 index 0000000000000..562134f5301e2 --- /dev/null +++ b/system/mrm_handler/launch/mrm_handler.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml new file mode 100644 index 0000000000000..e62091f2984e6 --- /dev/null +++ b/system/mrm_handler/package.xml @@ -0,0 +1,31 @@ + + + + mrm_handler + 0.1.0 + The mrm_handler ROS 2 package + Makoto Kurihara + Ryuta Kambe + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + nav_msgs + rclcpp + std_msgs + std_srvs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/mrm_handler/schema/mrm_handler.schema.json new file mode 100644 index 0000000000000..86b18449cb734 --- /dev/null +++ b/system/mrm_handler/schema/mrm_handler.schema.json @@ -0,0 +1,95 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for mrm handler", + "type": "object", + "definitions": { + "mrm_handler": { + "type": "object", + "properties": { + "update_rate": { + "type": "integer", + "description": "Timer callback period.", + "default": 10 + }, + "timeout_operation_mode_availability": { + "type": "number", + "description": "If the input `operation_mode_availability` topic cannot be received for more than `timeout_operation_mode_availability`, vehicle will make an emergency stop.", + "default": 0.5 + }, + "use_emergency_holding": { + "type": "boolean", + "description": "If this parameter is true, the handler does not recover to the NORMAL state.", + "default": false + }, + "timeout_emergency_recovery": { + "type": "number", + "description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.", + "default": 5.0 + }, + "timeout_takeover_request": { + "type": "number", + "description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.", + "default": 10.0 + }, + "use_takeover_request": { + "type": "boolean", + "description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.", + "default": "false" + }, + "use_parking_after_stopped": { + "type": "boolean", + "description": "If this parameter is true, it will publish PARKING shift command.", + "default": "false" + }, + "use_pull_over": { + "type": "boolean", + "description": "If this parameter is true, operate pull over when latent faults occur.", + "default": "false" + }, + "use_comfortable_stop": { + "type": "boolean", + "description": "If this parameter is true, operate comfortable stop when latent faults occur.", + "default": "false" + }, + "turning_hazard_on": { + "type": "object", + "properties": { + "emergency": { + "type": "boolean", + "description": "If this parameter is true, hazard lamps will be turned on during emergency state.", + "default": "true" + } + }, + "required": ["emergency"] + } + }, + "required": [ + "update_rate", + "timeout_operation_mode_availability", + "use_emergency_holding", + "timeout_emergency_recovery", + "timeout_takeover_request", + "use_takeover_request", + "use_parking_after_stopped", + "use_pull_over", + "use_comfortable_stop", + "turning_hazard_on" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/mrm_handler" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp new file mode 100644 index 0000000000000..976f5b3164abd --- /dev/null +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -0,0 +1,587 @@ +// 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 "mrm_handler/mrm_handler_core.hpp" + +#include +#include +#include +#include + +MrmHandler::MrmHandler() : Node("mrm_handler") +{ + // Parameter + param_.update_rate = declare_parameter("update_rate", 10); + param_.timeout_operation_mode_availability = + declare_parameter("timeout_operation_mode_availability", 0.5); + param_.use_emergency_holding = declare_parameter("use_emergency_holding", false); + param_.timeout_emergency_recovery = declare_parameter("timeout_emergency_recovery", 5.0); + param_.timeout_takeover_request = declare_parameter("timeout_takeover_request", 10.0); + param_.use_takeover_request = declare_parameter("use_takeover_request", false); + param_.use_parking_after_stopped = declare_parameter("use_parking_after_stopped", false); + param_.use_pull_over = declare_parameter("use_pull_over", false); + param_.use_comfortable_stop = declare_parameter("use_comfortable_stop", false); + param_.turning_hazard_on.emergency = declare_parameter("turning_hazard_on.emergency", true); + + using std::placeholders::_1; + + // Subscriber + sub_odom_ = create_subscription( + "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); + // subscribe control mode + sub_control_mode_ = create_subscription( + "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); + sub_operation_mode_availability_ = + create_subscription( + "~/input/operation_mode_availability", rclcpp::QoS{1}, + std::bind(&MrmHandler::onOperationModeAvailability, this, _1)); + sub_mrm_pull_over_status_ = create_subscription( + "~/input/mrm/pull_over/status", rclcpp::QoS{1}, + std::bind(&MrmHandler::onMrmPullOverStatus, this, _1)); + sub_mrm_comfortable_stop_status_ = create_subscription( + "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, + std::bind(&MrmHandler::onMrmComfortableStopStatus, this, _1)); + sub_mrm_emergency_stop_status_ = create_subscription( + "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, + std::bind(&MrmHandler::onMrmEmergencyStopStatus, this, _1)); + sub_operation_mode_state_ = create_subscription( + "~/input/api/operation_mode/state", rclcpp::QoS{1}, + std::bind(&MrmHandler::onOperationModeState, this, _1)); + + // Publisher + pub_hazard_cmd_ = create_publisher( + "~/output/hazard", rclcpp::QoS{1}); + pub_gear_cmd_ = + create_publisher("~/output/gear", rclcpp::QoS{1}); + pub_mrm_state_ = + create_publisher("~/output/mrm/state", rclcpp::QoS{1}); + + // Clients + client_mrm_pull_over_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_mrm_pull_over_ = create_client( + "~/output/mrm/pull_over/operate", rmw_qos_profile_services_default, + client_mrm_pull_over_group_); + client_mrm_comfortable_stop_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_mrm_comfortable_stop_ = create_client( + "~/output/mrm/comfortable_stop/operate", rmw_qos_profile_services_default, + client_mrm_comfortable_stop_group_); + client_mrm_emergency_stop_group_ = + create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + client_mrm_emergency_stop_ = create_client( + "~/output/mrm/emergency_stop/operate", rmw_qos_profile_services_default, + client_mrm_emergency_stop_group_); + + // Initialize + odom_ = std::make_shared(); + control_mode_ = std::make_shared(); + mrm_pull_over_status_ = std::make_shared(); + mrm_comfortable_stop_status_ = + std::make_shared(); + mrm_emergency_stop_status_ = std::make_shared(); + operation_mode_state_ = std::make_shared(); + mrm_state_.stamp = this->now(); + mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; + mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; + + // Timer + const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, std::bind(&MrmHandler::onTimer, this)); +} + +void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + odom_ = msg; +} + +void MrmHandler::onControlMode( + const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) +{ + control_mode_ = msg; +} + +void MrmHandler::onOperationModeAvailability( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg) +{ + stamp_operation_mode_availability_ = this->now(); + + if (!param_.use_emergency_holding) { + operation_mode_availability_ = msg; + return; + } + + if (!is_emergency_holding_) { + if (msg->autonomous) { + stamp_autonomous_become_unavailable_.reset(); + } else if (!msg->autonomous) { + if (!stamp_autonomous_become_unavailable_.has_value()) { + stamp_autonomous_become_unavailable_.emplace(this->now()); + } else { + const auto emergency_duration = + (this->now() - stamp_autonomous_become_unavailable_.value()).seconds(); + if (emergency_duration > param_.timeout_emergency_recovery) { + is_emergency_holding_ = true; + } + } + } + } + operation_mode_availability_ = msg; +} + +void MrmHandler::onMrmPullOverStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_pull_over_status_ = msg; +} + +void MrmHandler::onMrmComfortableStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_comfortable_stop_status_ = msg; +} + +void MrmHandler::onMrmEmergencyStopStatus( + const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) +{ + mrm_emergency_stop_status_ = msg; +} + +void MrmHandler::onOperationModeState( + const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) +{ + operation_mode_state_ = msg; +} + +autoware_auto_vehicle_msgs::msg::HazardLightsCommand MrmHandler::createHazardCmdMsg() +{ + using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + HazardLightsCommand msg; + + // Check emergency + const bool is_emergency = isEmergency(); + + if (is_emergency_holding_) { + // turn hazard on during emergency holding + msg.command = HazardLightsCommand::ENABLE; + } else if (is_emergency && param_.turning_hazard_on.emergency) { + // turn hazard on if vehicle is in emergency state and + // turning hazard on if emergency flag is true + msg.command = HazardLightsCommand::ENABLE; + } else { + msg.command = HazardLightsCommand::NO_COMMAND; + } + return msg; +} + +void MrmHandler::publishControlCommands() +{ + using autoware_auto_vehicle_msgs::msg::GearCommand; + + // Create timestamp + const auto stamp = this->now(); + + // Publish hazard command + pub_hazard_cmd_->publish(createHazardCmdMsg()); + + // Publish gear + { + GearCommand msg; + msg.stamp = stamp; + if (param_.use_parking_after_stopped && isStopped()) { + msg.command = GearCommand::PARK; + } else { + msg.command = GearCommand::DRIVE; + } + pub_gear_cmd_->publish(msg); + } +} + +void MrmHandler::publishMrmState() +{ + mrm_state_.stamp = this->now(); + pub_mrm_state_->publish(mrm_state_); +} + +void MrmHandler::operateMrm() +{ + using autoware_adapi_v1_msgs::msg::MrmState; + + if (mrm_state_.state == MrmState::NORMAL) { + // Cancel MRM behavior when returning to NORMAL state + const auto current_mrm_behavior = MrmState::NONE; + if (current_mrm_behavior != mrm_state_.behavior) { + cancelMrmBehavior(mrm_state_.behavior); + mrm_state_.behavior = current_mrm_behavior; + } + return; + } + if (mrm_state_.state == MrmState::MRM_OPERATING) { + const auto current_mrm_behavior = getCurrentMrmBehavior(); + if (current_mrm_behavior != mrm_state_.behavior) { + cancelMrmBehavior(mrm_state_.behavior); + callMrmBehavior(current_mrm_behavior); + mrm_state_.behavior = current_mrm_behavior; + } + return; + } + if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { + // TODO(mkuri): operate MRC behavior + // Do nothing + return; + } + if (mrm_state_.state == MrmState::MRM_FAILED) { + // Do nothing + return; + } + RCLCPP_WARN(this->get_logger(), "invalid MRM state: %d", mrm_state_.state); +} + +void MrmHandler::callMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +{ + using autoware_adapi_v1_msgs::msg::MrmState; + + auto request = std::make_shared(); + request->operate = true; + + if (mrm_behavior == MrmState::NONE) { + RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); + return; + } + if (mrm_behavior == MrmState::PULL_OVER) { + auto result = client_mrm_pull_over_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Pull over is operated"); + } else { + RCLCPP_ERROR(this->get_logger(), "Pull over failed to operate"); + } + return; + } + if (mrm_behavior == MrmState::COMFORTABLE_STOP) { + auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Comfortable stop is operated"); + } else { + RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to operate"); + } + return; + } + if (mrm_behavior == MrmState::EMERGENCY_STOP) { + auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Emergency stop is operated"); + } else { + RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to operate"); + } + return; + } + RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); +} + +void MrmHandler::cancelMrmBehavior( + const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior) const +{ + using autoware_adapi_v1_msgs::msg::MrmState; + + auto request = std::make_shared(); + request->operate = false; + + if (mrm_behavior == MrmState::NONE) { + // Do nothing + return; + } + if (mrm_behavior == MrmState::PULL_OVER) { + auto result = client_mrm_pull_over_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Pull over is canceled"); + } else { + RCLCPP_ERROR(this->get_logger(), "Pull over failed to cancel"); + } + return; + } + if (mrm_behavior == MrmState::COMFORTABLE_STOP) { + auto result = client_mrm_comfortable_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Comfortable stop is canceled"); + } else { + RCLCPP_ERROR(this->get_logger(), "Comfortable stop failed to cancel"); + } + return; + } + if (mrm_behavior == MrmState::EMERGENCY_STOP) { + auto result = client_mrm_emergency_stop_->async_send_request(request).get(); + if (result->response.success == true) { + RCLCPP_WARN(this->get_logger(), "Emergency stop is canceled"); + } else { + RCLCPP_ERROR(this->get_logger(), "Emergency stop failed to cancel"); + } + return; + } + RCLCPP_WARN(this->get_logger(), "invalid MRM behavior: %d", mrm_behavior); +} + +bool MrmHandler::isDataReady() +{ + if (!operation_mode_availability_) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for operation_mode_availability msg..."); + return false; + } + + if ( + param_.use_pull_over && + mrm_pull_over_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for mrm pull over to become available..."); + return false; + } + + if ( + param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == + tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for mrm comfortable stop to become available..."); + return false; + } + + if ( + mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + RCLCPP_INFO_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), + "waiting for mrm emergency stop to become available..."); + return false; + } + + return true; +} + +void MrmHandler::onTimer() +{ + if (!isDataReady()) { + return; + } + const bool is_operation_mode_availability_timeout = + (this->now() - stamp_operation_mode_availability_).seconds() > + param_.timeout_operation_mode_availability; + if (is_operation_mode_availability_timeout) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "heartbeat operation_mode_availability is timeout"); + mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING; + publishControlCommands(); + return; + } + + // Update Emergency State + updateMrmState(); + + // Publish control commands + publishControlCommands(); + operateMrm(); + publishMrmState(); +} + +void MrmHandler::transitionTo(const int new_state) +{ + using autoware_adapi_v1_msgs::msg::MrmState; + + const auto state2string = [](const int state) { + if (state == MrmState::NORMAL) { + return "NORMAL"; + } + if (state == MrmState::MRM_OPERATING) { + return "MRM_OPERATING"; + } + if (state == MrmState::MRM_SUCCEEDED) { + return "MRM_SUCCEEDED"; + } + if (state == MrmState::MRM_FAILED) { + return "MRM_FAILED"; + } + + const auto msg = "invalid state: " + std::to_string(state); + throw std::runtime_error(msg); + }; + + RCLCPP_INFO( + this->get_logger(), "MRM State changed: %s -> %s", state2string(mrm_state_.state), + state2string(new_state)); + + mrm_state_.state = new_state; +} + +void MrmHandler::updateMrmState() +{ + using autoware_adapi_v1_msgs::msg::MrmState; + using autoware_auto_vehicle_msgs::msg::ControlModeReport; + + // Check emergency + const bool is_emergency = isEmergency(); + + // Get mode + const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL; + + // State Machine + if (mrm_state_.state == MrmState::NORMAL) { + // NORMAL + if (is_auto_mode && is_emergency) { + if (param_.use_takeover_request) { + takeover_requested_time_ = this->get_clock()->now(); + is_takeover_request_ = true; + return; + } else { + transitionTo(MrmState::MRM_OPERATING); + return; + } + } + } else { + // Emergency + // Send recovery events if "not emergency" or "takeover done" + if (!is_emergency) { + transitionTo(MrmState::NORMAL); + return; + } + // TODO(TetsuKawa): Check if human can safely override, for example using DSM + if (is_takeover_done) { + transitionTo(MrmState::NORMAL); + return; + } + + if (is_takeover_request_) { + const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_; + if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) { + transitionTo(MrmState::MRM_OPERATING); + return; + } + } else if (mrm_state_.state == MrmState::MRM_OPERATING) { + // TODO(TetsuKawa): Check MRC is accomplished + if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (isStopped() && isArrivedAtGoal()) { + transitionTo(MrmState::MRM_SUCCEEDED); + return; + } + } else { + if (isStopped()) { + transitionTo(MrmState::MRM_SUCCEEDED); + return; + } + } + } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { + const auto current_mrm_behavior = getCurrentMrmBehavior(); + if (current_mrm_behavior != mrm_state_.behavior) { + transitionTo(MrmState::MRM_OPERATING); + } + } else if (mrm_state_.state == MrmState::MRM_FAILED) { + // Do nothing(only checking common recovery events) + } else { + const auto msg = "invalid state: " + std::to_string(mrm_state_.state); + throw std::runtime_error(msg); + } + } +} + +autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmBehavior() +{ + using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::OperationModeAvailability; + + // State machine + if (mrm_state_.behavior == MrmState::NONE) { + if (operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (operation_mode_availability_->comfortable_stop) { + if (param_.use_comfortable_stop) { + return MrmState::COMFORTABLE_STOP; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (operation_mode_availability_->comfortable_stop) { + if (param_.use_comfortable_stop) { + return MrmState::COMFORTABLE_STOP; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { + if (isStopped() && operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (operation_mode_availability_->comfortable_stop) { + if (param_.use_comfortable_stop) { + return MrmState::COMFORTABLE_STOP; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) { + if (isStopped() && operation_mode_availability_->pull_over) { + if (param_.use_pull_over) { + return MrmState::PULL_OVER; + } + } + if (!operation_mode_availability_->emergency_stop) { + RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); + } + return MrmState::EMERGENCY_STOP; + } + + return mrm_state_.behavior; +} + +bool MrmHandler::isStopped() +{ + constexpr auto th_stopped_velocity = 0.001; + if (odom_->twist.twist.linear.x < th_stopped_velocity) { + return true; + } + + return false; +} + +bool MrmHandler::isEmergency() const +{ + return !operation_mode_availability_->autonomous || is_emergency_holding_; +} + +bool MrmHandler::isArrivedAtGoal() +{ + using autoware_adapi_v1_msgs::msg::OperationModeState; + + return operation_mode_state_->mode == OperationModeState::STOP; +} diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp new file mode 100644 index 0000000000000..4aaab3296f64b --- /dev/null +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp @@ -0,0 +1,32 @@ +// 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 "mrm_handler/mrm_handler_core.hpp" + +#include + +#include + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::executors::MultiThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index aa8936d331a87..cc292c5a8c40f 100644 --- a/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -142,9 +142,6 @@ } }, "required": [ - "csv_path_accel_map", - "csv_path_brake_map", - "csv_path_steer_map", "convert_accel_cmd", "convert_brake_cmd", "convert_steer_cmd",