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 3ebf73dab7ba5..e4231a12a6add 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -73,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/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/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_control_rviz_plugin/src/tools/manual_controller.cpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp index fdd270fcce2ef..8bbb096f728ec 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.cpp @@ -30,14 +30,6 @@ using std::placeholders::_1; namespace rviz_plugins { -double lowpassFilter( - const double current_value, const double prev_value, double cutoff, const double dt) -{ - const double tau = 1.0 / (2.0 * M_PI * cutoff); - const double a = tau / (dt + tau); - return prev_value * a + (1.0 - a) * current_value; -} - ManualController::ManualController(QWidget * parent) : rviz_common::Panel(parent) { auto * state_layout = new QHBoxLayout; @@ -115,25 +107,23 @@ void ManualController::update() ackermann.stamp = raw_node_->get_clock()->now(); ackermann.lateral.steering_tire_angle = steering_angle_; ackermann.longitudinal.speed = cruise_velocity_; - if (current_acceleration_) { - /** - * @brief Calculate desired acceleration by simple BackSteppingControl - * V = 0.5*(v-v_des)^2 >= 0 - * D[V] = (D[v] - a_des)*(v-v_des) <=0 - * a_des = k_const *(v - v_des) + a (k < 0 ) - */ - const double k = -0.5; - const double v = current_velocity_; - const double v_des = cruise_velocity_; - const double a = *current_acceleration_; - const double a_des = k * (v - v_des) + a; - ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); - } + /** + * @brief Calculate desired acceleration by simple BackSteppingControl + * V = 0.5*(v-v_des)^2 >= 0 + * D[V] = (D[v] - a_des)*(v-v_des) <=0 + * a_des = k_const *(v - v_des) + a (k < 0 ) + */ + const double k = -0.5; + const double v = current_velocity_; + const double v_des = cruise_velocity_; + const double a = current_acceleration_; + const double a_des = k * (v - v_des) + a; + ackermann.longitudinal.acceleration = std::clamp(a_des, -1.0, 1.0); } GearCommand gear_cmd; { const double eps = 0.001; - if (ackermann.longitudinal.speed > eps) { + if (ackermann.longitudinal.speed > eps && current_velocity_ > -eps) { gear_cmd.command = GearCommand::DRIVE; } else if (ackermann.longitudinal.speed < -eps && current_velocity_ < eps) { gear_cmd.command = GearCommand::REVERSE; @@ -220,19 +210,11 @@ void ManualController::onEngageStatus(const Engage::ConstSharedPtr msg) void ManualController::onVelocity(const VelocityReport::ConstSharedPtr msg) { current_velocity_ = msg->longitudinal_velocity; - if (previous_velocity_) { - const double cutoff = 10.0; - const double dt = 1.0 / 10.0; - const double acc = (current_velocity_ - *previous_velocity_) / dt; - if (!current_acceleration_) { - current_acceleration_ = std::make_unique(acc); - } else { - current_acceleration_ = - std::make_unique(lowpassFilter(acc, *current_acceleration_, cutoff, dt)); - } - } - previous_velocity_ = std::make_unique(msg->longitudinal_velocity); - prev_stamp_ = rclcpp::Time(msg->header.stamp); +} + +void ManualController::onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg) +{ + current_acceleration_ = msg->accel.accel.linear.x; } void ManualController::onGear(const GearReport::ConstSharedPtr msg) diff --git a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp index dee6f9a7aba21..aaa625bff911e 100644 --- a/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp +++ b/common/tier4_control_rviz_plugin/src/tools/manual_controller.hpp @@ -25,6 +25,7 @@ #include #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" #include #include @@ -40,6 +41,7 @@ namespace rviz_plugins using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; +using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using tier4_control_msgs::msg::GateMode; using EngageSrv = tier4_external_api_msgs::srv::Engage; @@ -67,6 +69,7 @@ public Q_SLOTS: // NOLINT for Qt void onPublishControlCommand(); void onGateMode(const GateMode::ConstSharedPtr msg); void onVelocity(const VelocityReport::ConstSharedPtr msg); + void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onEngageStatus(const Engage::ConstSharedPtr msg); void onGear(const GearReport::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; @@ -82,9 +85,7 @@ public Q_SLOTS: // NOLINT for Qt double cruise_velocity_{0.0}; double steering_angle_{0.0}; double current_velocity_{0.0}; - rclcpp::Time prev_stamp_; - std::unique_ptr previous_velocity_; - std::unique_ptr current_acceleration_; + double current_acceleration_{0.0}; QLabel * gate_mode_label_ptr_; QLabel * gear_label_ptr_; 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/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_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 9d3fba05b4a6e..7f2f446ae1bee 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -17,15 +17,15 @@ - - + + - + diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index c00c90d467090..3042774d16fd3 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -109,7 +109,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index aeba276308646..63dac42f2063e 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -3,12 +3,12 @@ - + - + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index 99074d4ae4a98..09d61823235d8 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -40,7 +40,7 @@ def __init__(self, context): self.ground_segmentation_param = yaml.safe_load(f)["/**"]["ros__parameters"] self.single_frame_obstacle_seg_output = ( - "/perception/obstacle_segmentation/single_frame/pointcloud_raw" + "/perception/obstacle_segmentation/single_frame/pointcloud" ) self.output_topic = "/perception/obstacle_segmentation/pointcloud" self.use_single_frame_filter = self.ground_segmentation_param["use_single_frame_filter"] @@ -297,7 +297,7 @@ def create_time_series_outlier_filter_components(input_topic, output_topic): ComposableNode( package="occupancy_grid_map_outlier_filter", plugin="occupancy_grid_map_outlier_filter::OccupancyGridMapOutlierFilterComponent", - name="occupancy_grid_map_outlier_filter", + name="occupancy_grid_based_outlier_filter", remappings=[ ("~/input/occupancy_grid_map", "/perception/occupancy_grid_map/map"), ("~/input/pointcloud", input_topic), diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index dbb74335f79d2..c6213ee313c2b 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -139,7 +139,7 @@ - + @@ -247,4 +247,9 @@ + + + + + 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/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/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/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/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md index 6f4d4e22aa26e..cb86ebe7eea85 100644 --- a/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md +++ b/perception/probabilistic_occupancy_grid_map/synchronized_grid_map_fusion.md @@ -97,7 +97,7 @@ You need to generate OGMs in each sensor frame before achieving grid map fusion. ```xml - + 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/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/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/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/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml index cd2a51851fe5f..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 diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 472870e2f36fc..0e2aaed2b38f6 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -208,11 +208,6 @@ bool LaneChangeInterface::canTransitSuccessState() } } - if (!module_type_->isValidPath()) { - log_debug_throttled("Has no valid path."); - return true; - } - if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { log_debug_throttled("Abort process has completed."); return true; 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_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/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index aaf711b855e7a..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 @@ -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. * diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_start_planner_module/src/manager.cpp index bd96a095eb767..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 = @@ -356,6 +377,33 @@ void StartPlannerModuleManager::updateModuleParams( 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( @@ -390,6 +438,9 @@ void StartPlannerModuleManager::updateModuleParams( 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( 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 d21a68048e34f..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 @@ -74,13 +74,27 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - // check lane departure - // The method for lane departure checking verifies if the footprint of each point on the path is - // contained within a lanelet using `boost::geometry::within`, which incurs a high computational - // cost. const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + // if 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 ( - parameters_.check_shift_path_lane_departure && + is_lane_departure_check_required && lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) { continue; } 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 ed81f39135181..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 @@ -182,18 +182,14 @@ void StartPlannerModule::updateData() 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 @@ -364,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; @@ -459,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( @@ -480,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(); @@ -569,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( @@ -590,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(); @@ -658,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; } @@ -682,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 = @@ -1050,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; } @@ -1099,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; } @@ -1197,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; } @@ -1389,22 +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; + 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; + } } } 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_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 9f18a1a6de816..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. @@ -323,8 +326,6 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); - planner_data_.has_received_signal_ = true; - // clear previous observation planner_data_.traffic_light_id_map_raw_.clear(); const auto traffic_light_id_map_last_observed_old = 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 23997e34fbfea..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,8 +83,9 @@ struct PlannerData std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; - // this value becomes true once the signal message is received - bool has_received_signal_ = false; + // 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_; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 946741b14cf78..413ce78beacee 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -281,16 +281,15 @@ bool TrafficLightModule::isStopSignal() { updateTrafficSignal(); - // Pass through if no traffic signal information has been received yet - // This is to prevent stopping on the planning simulator - if (!planner_data_->has_received_signal_) { - return false; - } - - // Stop if there is no upcoming traffic signal information - // This is to safely stop in cases such that traffic light recognition is not working properly or - // the map is incorrect + // 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_) { + if (planner_data_->is_simulation) { + return false; + } return true; } diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 088c39ab67fbb..e9a0108cdaa24 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -27,6 +27,7 @@ #include #include +#include namespace mission_planner { @@ -132,6 +133,8 @@ 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_); } Pose MissionPlanner::transform_pose(const Pose & pose, const Header & header) @@ -394,7 +397,9 @@ LaneletRoute MissionPlanner::create_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; } @@ -413,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; @@ -534,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); @@ -546,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 04c1de79d5b6d..8016851d5a7d3 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -93,6 +93,7 @@ class MissionPlanner : public rclcpp::Node 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); 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/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/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/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(