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/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index aa1dcfcd1651d..ba961b9ac5b00 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -502,6 +502,11 @@ void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg) style_sheet = "background-color: #00FF00;"; // green break; + case MRMState::PULL_OVER: + text = "PULL_OVER"; + style_sheet = "background-color: #FFFF00;"; // yellow + break; + case MRMState::COMFORTABLE_STOP: text = "COMFORTABLE_STOP"; style_sheet = "background-color: #FFFF00;"; // yellow 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/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 16396ef2961d8..c6213ee313c2b 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -139,7 +139,7 @@ - + diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 950457f0d49b1..31d1c6588165f 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -152,18 +152,14 @@ bool MapUpdateModule::update_ndt(const geometry_msgs::msg::Point & position, Ndt } const auto exe_start_time = std::chrono::system_clock::now(); - const size_t add_size = maps_to_add.size(); // Perform heavy processing outside of the lock scope - std::vector>> points_pcl(add_size); - - for (size_t i = 0; i < add_size; i++) { - points_pcl[i] = pcl::make_shared>(); - pcl::fromROSMsg(maps_to_add[i].pointcloud, *points_pcl[i]); - } // Add pcd - for (size_t i = 0; i < add_size; i++) { - ndt.addTarget(points_pcl[i], maps_to_add[i].cell_id); + for (auto & map : maps_to_add) { + auto cloud = pcl::make_shared>(); + + pcl::fromROSMsg(map.pointcloud, *cloud); + ndt.addTarget(cloud, map.cell_id); } // Remove pcd 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_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index a52c96567e928..6c9254919cac7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -34,6 +34,9 @@ lanelet::LineString3d getLineStringFromArcLength( lanelet::Points3d points; double accumulated_length = 0; size_t start_index = linestring.size(); + if (start_index == 0) { + return lanelet::LineString3d{lanelet::InvalId, points}; + } for (size_t i = 0; i < linestring.size() - 1; i++) { const auto & p1 = linestring[i]; const auto & p2 = linestring[i + 1]; 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/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(