diff --git a/localization/localization_error_monitor/src/diagnostics.cpp b/localization/localization_error_monitor/src/diagnostics.cpp index 0c5a4a7800639..375fccfa06787 100644 --- a/localization/localization_error_monitor/src/diagnostics.cpp +++ b/localization/localization_error_monitor/src/diagnostics.cpp @@ -23,7 +23,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracy( diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy"; + key_value.key = "localization_error_ellipse"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); @@ -47,7 +47,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkLocalizationAccuracyLateralDirection diagnostic_msgs::msg::DiagnosticStatus stat; diagnostic_msgs::msg::KeyValue key_value; - key_value.key = "localization_accuracy_lateral_direction"; + key_value.key = "localization_error_ellipse_lateral_direction"; key_value.value = std::to_string(ellipse_size); stat.values.push_back(key_value); diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 9ed871c6ac401..6368305bdbfad 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -32,11 +32,7 @@ It estimates the height and tilt of the ground from lanelet2. ### Parameters -| Name | Type | Description | -| ----------------- | ---- | -------------------------------------------------------- | -| `force_zero_tilt` | bool | if true, the tilt is always determined to be horizontal. | -| `K` | int | parameter for nearest k search | -| `R` | int | parameter for radius search | +{{ json_to_markdown("localization/yabloc/yabloc_common/schema/ground_server.schema.json") }} ## ll2_decomposer diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index 921a9277a727c..9816a02c48121 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -53,15 +53,7 @@ This node extract road surface region by [graph-based-segmentation](https://docs ### Parameters -| Name | Type | Description | -| --------------------------------- | ------ | ------------------------------------------------------------------ | -| `target_height_ratio` | double | height on the image to retrieve the candidate road surface | -| `target_candidate_box_width` | int | size of the square area to search for candidate road surfaces | -| `pickup_additional_graph_segment` | bool | if this is true, additional regions of similar color are retrieved | -| `similarity_score_threshold` | double | threshold for picking up additional areas | -| `sigma` | double | parameters for cv::ximgproc::segmentation | -| `k` | double | parameters for cv::ximgproc::segmentation | -| `min_size` | double | parameters for cv::ximgproc::segmentation | +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/graph_segment.schema.json") }} ## segment_filter @@ -89,7 +81,7 @@ This is a node that integrates the results of graph_segment and lsd to extract r ### Parameters -{{ json_to_markdown("localization/yabloc/yabloc_common/schema/segment_filter.schema.json") }} +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/segment_filter.schema.json") }} ## undistort @@ -120,7 +112,7 @@ This is to avoid redundant decompression within Autoware. ### Parameters -{{ json_to_markdown("localization/yabloc/yabloc_common/schema/undistort.schema.json") }} +{{ json_to_markdown("localization/yabloc/yabloc_image_processing/schema/undistort.schema.json") }} #### about tf_static overriding diff --git a/perception/cluster_merger/CMakeLists.txt b/perception/cluster_merger/CMakeLists.txt index 49506f4b439fb..5ad0de572b44f 100644 --- a/perception/cluster_merger/CMakeLists.txt +++ b/perception/cluster_merger/CMakeLists.txt @@ -24,4 +24,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/cluster_merger/config/cluster_merger.param.yaml b/perception/cluster_merger/config/cluster_merger.param.yaml new file mode 100644 index 0000000000000..adca6c203282f --- /dev/null +++ b/perception/cluster_merger/config/cluster_merger.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + output_frame_id: "base_link" diff --git a/perception/cluster_merger/launch/cluster_merger.launch.xml b/perception/cluster_merger/launch/cluster_merger.launch.xml index 1bbd0ebd91e12..f0f90efe051a0 100644 --- a/perception/cluster_merger/launch/cluster_merger.launch.xml +++ b/perception/cluster_merger/launch/cluster_merger.launch.xml @@ -3,12 +3,12 @@ - + - + diff --git a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp index b43102ecbd16d..73cabd00592a8 100644 --- a/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_dynamic_avoidance_module/src/scene.cpp @@ -397,6 +397,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + output.modified_goal = getPreviousModuleOutput().modified_goal; return output; } diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index f991ca0d849f0..8d2e3b451fe98 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -55,7 +55,7 @@ class NormalLaneChange : public LaneChangeBase BehaviorModuleOutput generateOutput() override; - void extendOutputDrivableArea(BehaviorModuleOutput & output) override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; @@ -65,7 +65,7 @@ class NormalLaneChange : public LaneChangeBase void resetParameters() override; - TurnSignalInfo updateOutputTurnSignal() override; + TurnSignalInfo updateOutputTurnSignal() const override; bool calcAbortPath() override; @@ -141,7 +141,7 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() override; + TurnSignalInfo calcTurnSignalInfo() const override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 400d5505dc49f..e78d706334bae 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -66,7 +66,7 @@ class LaneChangeBase virtual BehaviorModuleOutput generateOutput() = 0; - virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) const = 0; virtual PathWithLaneId getReferencePath() const = 0; @@ -74,7 +74,7 @@ class LaneChangeBase virtual void resetParameters() = 0; - virtual TurnSignalInfo updateOutputTurnSignal() = 0; + virtual TurnSignalInfo updateOutputTurnSignal() const = 0; virtual bool hasFinishedLaneChange() const = 0; @@ -225,7 +225,7 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() = 0; + virtual TurnSignalInfo calcTurnSignalInfo() const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index f5100f16129c2..b115ab7c163c0 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -183,7 +183,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() return output; } -void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -431,7 +431,7 @@ void NormalLaneChange::resetParameters() RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } -TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() +TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( @@ -1447,7 +1447,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return safety_status; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() +TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const { const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { double accumulated_length = 0.0; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index cbcec2e3095d3..4caf5683f3982 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -630,32 +630,38 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrisKeepLast(); + const auto get_sorted_keep_last_modules = [this](const auto & modules) { + std::vector keep_last_modules; + + std::copy_if( + modules.begin(), modules.end(), std::back_inserter(keep_last_modules), + [this](const auto & m) { return getManager(m)->isKeepLast(); }); + + // sort by priority (low -> high) + std::sort( + keep_last_modules.begin(), keep_last_modules.end(), [this](const auto & a, const auto & b) { + return getManager(a)->getPriority() < getManager(b)->getPriority(); + }); + + return keep_last_modules; }; - move_to_end(approved_module_ptrs_, keep_last_module_cond); + + for (const auto & module : get_sorted_keep_last_modules(approved_module_ptrs_)) { + move_to_end(approved_module_ptrs_, module); + } } // lock approved modules besides last one @@ -768,6 +774,25 @@ BehaviorModuleOutput PlannerManager::runApprovedModules(const std::shared_ptrgetCurrentStatus() == ModuleStatus::SUCCESS; }; diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 03fae6864fe50..4e25e9257ddfd 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1884,6 +1884,10 @@ DrivableAreaInfo combineDrivableAreaInfo( drivable_area_info1.enable_expanding_intersection_areas || drivable_area_info2.enable_expanding_intersection_areas; + // drivable margin + combined_drivable_area_info.drivable_margin = + std::max(drivable_area_info1.drivable_margin, drivable_area_info2.drivable_margin); + return combined_drivable_area_info; } diff --git a/planning/behavior_path_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 5bbde1c2fc523..cffce8218c554 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 @@ -139,7 +139,7 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override; + bool canTransitIdleToRunningState() override { return true; } /** * @brief init member variables. 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 6c9000caac956..8937c8a837694 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 @@ -347,11 +347,6 @@ bool StartPlannerModule::canTransitSuccessState() return hasFinishedPullOut(); } -bool StartPlannerModule::canTransitIdleToRunningState() -{ - return isActivated() && !isWaitingApproval(); -} - BehaviorModuleOutput StartPlannerModule::plan() { if (isWaitingApproval()) { @@ -1317,6 +1312,7 @@ bool StartPlannerModule::planFreespacePath() void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { if (status_.planner_type == PlannerType::FREESPACE) { + std::cerr << "Freespace planner updated drivable area." << std::endl; const double drivable_area_margin = planner_data_->parameters.vehicle_width; output.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index ce231659ccf78..c1ddc28e03426 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -196,7 +196,7 @@ In the `stuck_vehicle` namespace, the following parameters are defined. | ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | | `stuck_vehicle_velocity` | [m/s] | double | maximum velocity threshold whether the target vehicle is stopped or not | | `max_stuck_vehicle_lateral_offset` | [m] | double | maximum lateral offset of the target vehicle position | -| `stuck_vehicle_attention_range` | [m] | double | detection area length ahead of the crosswalk | +| `required_clearance` | [m] | double | clearance to be secured between the ego and the ahead vehicle | | `min_acc` | [m/ss] | double | minimum acceleration to stop | | `min_jerk` | [m/sss] | double | minimum jerk to stop | | `max_jerk` | [m/sss] | double | maximum jerk to stop | diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg index c517be5bb9967..773edf7989de9 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg @@ -1,92 +1,93 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -
+
-
- stuck_vehicle_attention_range +
+ vehicle_length+required_clearance
- stuck_vehicle_atten... + vehicle_length+required_clearance - - - - - - - - - + + + + + + + + + + -
+
-
+
max_stuck_vehicle_lateral_offset
- max_stuck_vehicle_l... + max_stuck_vehicle_lateral_offset - - + + - + Text is not SVG - cannot display diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 4bb0d8a2883dd..9792aa2bdd60b 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -8,6 +8,7 @@ Fumiya Watanabe Takamasa Horibe Makoto Kurihara + Satoshi Ota Apache License 2.0 Takamasa Horibe diff --git a/simulator/fault_injection/config/fault_injection.param.yaml b/simulator/fault_injection/config/fault_injection.param.yaml index 1a57b852f7361..ac02442d70b4d 100644 --- a/simulator/fault_injection/config/fault_injection.param.yaml +++ b/simulator/fault_injection/config/fault_injection.param.yaml @@ -4,7 +4,7 @@ vehicle_is_out_of_lane: "lane_departure" trajectory_deviation_is_high: "trajectory_deviation" localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_accuracy" + localization_accuracy_is_low: "localization_error_ellipse" map_version_is_different: "map_version" trajectory_is_invalid: "trajectory_point_validation" cpu_temperature_is_high: "CPU Temperature" diff --git a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml index 55af6ab2d2c55..9ed82304036cc 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/localization.param.yaml @@ -24,9 +24,9 @@ contains: ["ndt_scan_matcher"] timeout: 1.0 - localization_accuracy: + localization_error_ellipse: type: diagnostic_aggregator/GenericAnalyzer - path: localization_accuracy + path: localization_error_ellipse contains: ["localization: localization_error_monitor"] timeout: 1.0 diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 1778f6594f0c3..2b58f5b42c0be 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/localization/performance_monitoring/localization_accuracy: default + /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 9784259490ec2..f1031444394d0 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -24,7 +24,7 @@ /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/localization/performance_monitoring/localization_accuracy: default + # /autoware/localization/performance_monitoring/localization_error_ellipse: default /autoware/map/node_alive_monitoring: default