From b32f8ef5f1791c268aab7265937e87f8eb23ad69 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Sun, 1 Oct 2023 17:06:10 +0300 Subject: [PATCH 001/206] feat(tier4_control_launch): add launch argument for predicted path checker (#5186) Signed-off-by: Berkay Karaman --- launch/tier4_control_launch/launch/control.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index af5b82349f180..06718a2cf0ffa 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -404,6 +404,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("external_cmd_selector_param_path") add_launch_arg("aeb_param_path") add_launch_arg("predicted_path_checker_param_path") + add_launch_arg("enable_predicted_path_checker") add_launch_arg("enable_autonomous_emergency_braking") add_launch_arg("check_external_emergency_heartbeat") From c99c6b2fc7062e6e21edb80a1b96797f59d04efb Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 2 Oct 2023 10:55:00 +0900 Subject: [PATCH 002/206] feat(lane_change): expand target lanes for object filtering (#5167) * feat(lane_change): expand target lanes for object filtering Signed-off-by: Muhammad Zulfaqar Azmi * use expanded target lanes in collision check Signed-off-by: Muhammad Zulfaqar Azmi --------- Signed-off-by: Muhammad Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 5 +++++ .../lane_change/lane_change_module_data.hpp | 2 ++ .../utils/lane_change/utils.hpp | 19 +++++++++++++++++++ .../src/scene_module/lane_change/manager.cpp | 5 ++++- .../src/scene_module/lane_change/normal.cpp | 16 ++++++++++++---- .../src/utils/lane_change/utils.cpp | 9 +++++++++ 6 files changed, 51 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 583bdeaaf8dc5..fda7390abdb9e 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -38,6 +38,11 @@ longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + # lane expansion for object filtering + lane_expansion: + left_offset: 0.0 # [m] + right_offset: 0.0 # [m] + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 27735777c27cc..0ba35635a9143 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -67,6 +67,8 @@ struct LaneChangeParameters bool check_objects_on_current_lanes{true}; bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; + double lane_expansion_left_offset{0.0}; + double lane_expansion_right_offset{0.0}; // regulatory elements bool regulate_on_crosswalk{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 8a78d2665fa8f..b2592bc1b899b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -186,5 +186,24 @@ ExtendedPredictedObject transform( bool isCollidedPolygonsInLanelet( const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + +/** + * @brief Generates expanded lanelets based on the given direction and offsets. + * + * Expands the provided lanelets in either the left or right direction based on + * the specified direction. If the direction is 'LEFT', the lanelets are expanded + * using the left_offset; if 'RIGHT', they are expanded using the right_offset. + * Otherwise, no expansion occurs. + * + * @param lanes The lanelets to be expanded. + * @param direction The direction of expansion: either LEFT or RIGHT. + * @param left_offset The offset value for left expansion. + * @param right_offset The offset value for right expansion. + * @return lanelet::ConstLanelets A collection of expanded lanelets. + */ +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset); + } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index a502c9d8e43ec..9c45f79d00263 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -77,7 +77,10 @@ LaneChangeModuleManager::LaneChangeModuleManager( getOrDeclareParameter(*node, parameter("check_objects_on_other_lanes")); p.use_all_predicted_path = getOrDeclareParameter(*node, parameter("use_all_predicted_path")); - + p.lane_expansion_left_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.left_offset")); + p.lane_expansion_right_offset = + getOrDeclareParameter(*node, parameter("safety_check.lane_expansion.right_offset")); // lane change regulations p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); p.regulate_on_intersection = diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 1bcd3e6b0cdcd..d8fa9cb5630e9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -674,8 +674,11 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_polygon = utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); - const auto target_polygon = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + const auto target_polygon = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); std::vector> target_backward_polygons; @@ -1449,6 +1452,11 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( target_objects.other_lane.end()); } + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lane_change_path.info.target_lanes, direction_, + lane_change_parameters_->lane_expansion_left_offset, + lane_change_parameters_->lane_expansion_right_offset); + for (const auto & obj : collision_check_objects) { auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( @@ -1464,8 +1472,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( collided_polygons, lane_change_path.info.current_lanes); - const auto collision_in_target_lanes = utils::lane_change::isCollidedPolygonsInLanelet( - collided_polygons, lane_change_path.info.target_lanes); + const auto collision_in_target_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { continue; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 0ae599a74e439..df609dce48512 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -1104,4 +1104,13 @@ bool isCollidedPolygonsInLanelet( return std::any_of(collided_polygons.begin(), collided_polygons.end(), is_in_lanes); } + +lanelet::ConstLanelets generateExpandedLanelets( + const lanelet::ConstLanelets & lanes, const Direction direction, const double left_offset, + const double right_offset) +{ + const auto left_extend_offset = (direction == Direction::LEFT) ? left_offset : 0.0; + const auto right_extend_offset = (direction == Direction::RIGHT) ? -right_offset : 0.0; + return lanelet::utils::getExpandedLanelets(lanes, left_extend_offset, right_extend_offset); +} } // namespace behavior_path_planner::utils::lane_change From de83f251d3e9bc9399530107df5ab4e938c3413f Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 2 Oct 2023 11:37:47 +0900 Subject: [PATCH 003/206] fix(crosswalk_traffic_light_estimator): change the shape of pedestrian light to CIRCLE (#5168) Signed-off-by: Tomohito Ando --- perception/crosswalk_traffic_light_estimator/src/node.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index e45e3494dadce..870b8bc7c13f5 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -231,6 +231,7 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( TrafficSignal output_traffic_signal; TrafficSignalElement output_traffic_signal_element; output_traffic_signal_element.color = color; + output_traffic_signal_element.shape = TrafficSignalElement::CIRCLE; output_traffic_signal_element.confidence = 1.0; output_traffic_signal.elements.push_back(output_traffic_signal_element); output_traffic_signal.traffic_signal_id = tl_reg_elem->id(); From 3c5a0b84d195a60f045c5e829024d085c77735a2 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Mon, 2 Oct 2023 11:38:20 +0900 Subject: [PATCH 004/206] fix(crosswalk): check all elements in traffic signal (#5169) Signed-off-by: Tomohito Ando --- .../src/scene_crosswalk.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 9ea03996c88dd..b99bd522b81f8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -962,8 +962,10 @@ bool CrosswalkModule::isRedSignalForPedestrians() const continue; } - if (lights.front().color == TrafficSignalElement::RED) { - return true; + for (const auto & element : lights) { + if ( + element.color == TrafficSignalElement::RED && element.shape == TrafficSignalElement::CIRCLE) + return true; } } From a44830788f70b4e84ccdd874d56eea2e84d7ca1b Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 2 Oct 2023 12:06:24 +0900 Subject: [PATCH 005/206] feat(behavior_path_planner): safety check against dynamic objects after approval for start_planner (#5056) * add stop judgement after approval Signed-off-by: kyoichi-sugahara * update param Signed-off-by: kyoichi-sugahara * add status of has_stop_point Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../goal_planner/goal_planner.param.yaml | 10 ++-- .../start_planner/start_planner.param.yaml | 10 ++-- .../start_planner/start_planner_module.hpp | 3 ++ .../goal_planner/goal_planner_module.cpp | 7 +-- .../start_planner/start_planner_module.cpp | 51 ++++++++++++++++--- 5 files changed, 59 insertions(+), 22 deletions(-) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 0ca028e053fbc..5ffac49847ce6 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -132,7 +132,7 @@ # detection range object_check_forward_distance: 10.0 object_check_backward_distance: 100.0 - ignore_object_velocity_threshold: 0.0 + ignore_object_velocity_threshold: 1.0 # ObjectTypesToCheck object_types_to_check: check_car: true @@ -164,11 +164,11 @@ check_all_predicted_path: true publish_debug_marker: false rss_params: - rear_vehicle_reaction_time: 1.0 + rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 1.0 - longitudinal_distance_min_threshold: 1.0 - longitudinal_velocity_delta_time: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 # temporary diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 6760ec787bb03..4a42da9bc5ac3 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -95,7 +95,7 @@ # detection range object_check_forward_distance: 10.0 object_check_backward_distance: 100.0 - ignore_object_velocity_threshold: 0.0 + ignore_object_velocity_threshold: 1.0 # ObjectTypesToCheck object_types_to_check: check_car: true @@ -127,11 +127,11 @@ check_all_predicted_path: true publish_debug_marker: false rss_params: - rear_vehicle_reaction_time: 1.0 + rear_vehicle_reaction_time: 2.0 rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 1.0 - longitudinal_distance_min_threshold: 1.0 - longitudinal_velocity_delta_time: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # hysteresis factor to expand/shrink polygon hysteresis_factor_expand_rate: 1.0 # temporary diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 9bbd1203ee6b0..7ab404d99bb12 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -67,6 +67,9 @@ struct PullOutStatus false}; // after backward driving is complete, this is set to true (warning: this is set to // false at next cycle after backward driving is complete) Pose pull_out_start_pose{}; + bool prev_is_safe_dynamic_objects{false}; + std::shared_ptr prev_stop_path_after_approval{nullptr}; + bool has_stop_point{false}; PullOutStatus() {} }; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 5d9058890f351..574a1a38fc66a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -365,12 +365,7 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { - // TODO(Sugahara): should check safe or not but in the current flow, it is not possible. - if (status_.pull_over_path == nullptr) { - return true; - } - - if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 819686c20ff54..092b91499252c 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -136,6 +136,12 @@ void StartPlannerModule::updateData() if (has_received_new_route) { status_ = PullOutStatus(); } + // check safety status after back finished + if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) { + status_.is_safe_dynamic_objects = isSafePath(); + } else { + status_.is_safe_dynamic_objects = true; + } } bool StartPlannerModule::isExecutionRequested() const @@ -179,17 +185,19 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { + // when is_safe_static_objects is false,the path is not generated and approval shouldn't be + // allowed if (!status_.is_safe_static_objects) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against static objects"); return false; } - if (status_.pull_out_path.partial_paths.empty()) { - return true; - } - - if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) { + if ( + parameters_->safety_check_params.enable_safety_check && status_.back_finished && + isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); + stop_pose_ = planner_data_->self_odometry->pose.pose; return false; } } @@ -221,6 +229,7 @@ void StartPlannerModule::updateCurrentState() if (status_.backward_driving_complete) { current_state_ = ModuleStatus::SUCCESS; // for breaking loop } + print(magic_enum::enum_name(from), magic_enum::enum_name(current_state_)); } @@ -252,12 +261,42 @@ BehaviorModuleOutput StartPlannerModule::plan() } PathWithLaneId path; + + // Check if backward motion is finished if (status_.back_finished) { + // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); incrementPathIndex(); } - path = getCurrentPath(); + + if (!status_.is_safe_dynamic_objects && !isWaitingApproval() && !status_.has_stop_point) { + auto current_path = getCurrentPath(); + const auto stop_path = + behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + parameters_->maximum_jerk_for_stop); + + // Insert stop point in the path if needed + if (stop_path) { + RCLCPP_ERROR_THROTTLE( + getLogger(), *clock_, 5000, "Insert stop point in the path because of dynamic objects"); + path = *stop_path; + status_.prev_stop_path_after_approval = std::make_shared(path); + status_.has_stop_point = true; + } else { + path = current_path; + } + } else if (!isWaitingApproval() && status_.has_stop_point) { + // Delete stop point if conditions are met + if (status_.is_safe_dynamic_objects && isStopped()) { + status_.has_stop_point = false; + path = getCurrentPath(); + } + path = *status_.prev_stop_path_after_approval; + } else { + path = getCurrentPath(); + } } else { path = status_.backward_path; } From 14ef695b33325ac946438063278a16d1835c18f0 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Oct 2023 12:11:33 +0900 Subject: [PATCH 006/206] fix(trajectory_visualizer): fix plotter error (#5181) fix(trajectory_visualizer): fix plotter Signed-off-by: Mamoru Sobue --- .../planning_debug_tools/scripts/trajectory_visualizer.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 73e41b052cd90..10bd41c5c9c93 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -178,15 +178,15 @@ def __init__(self): # main process if PLOT_TYPE == "VEL_ACC_JERK": + self.setPlotTrajectory() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectory, interval=100, blit=True ) - self.setPlotTrajectory() else: + self.setPlotTrajectoryVelocity() self.ani = animation.FuncAnimation( self.fig, self.plotTrajectoryVelocity, interval=100, blit=True ) - self.setPlotTrajectoryVelocity() plt.show() From 93b61184520d93ab8bcdb653f51d94a2de5bfbf9 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 2 Oct 2023 15:00:43 +0900 Subject: [PATCH 007/206] feat(intersection): ignore occlusion behind blocking vehicle (#5122) Signed-off-by: Mamoru Sobue --- .../README.md | 10 + .../docs/occlusion_grid.drawio.svg | 2616 +++++++++++++++++ .../src/debug.cpp | 6 + .../src/scene_intersection.cpp | 87 +- .../src/util.cpp | 19 +- .../src/util.hpp | 7 +- .../src/util_type.hpp | 1 + 7 files changed, 2714 insertions(+), 32 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 5f26ef34c1186..e8a823a7e790e 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -94,6 +94,16 @@ If a stopline is associated with the intersection lane, that line is used as the To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +### Occlusion detection + +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough the ego vehicle will once stop at the _default stop line_ for `occlusion.before_creep_stop_time`, and then slowly creep toward _occlusion peeking stop line_ at the speed of `occlusion.occlusion_creep_velocity` if `occlusion.enable_creeping` is true. During the creeping if collision is detected this module inserts a stop line instantly, and if the FOV gets sufficiently clear the _intersection occlusion_ wall will disappear. If occlusion is cleared and no collision is detected the ego vehicle will pass the intersection. + +The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from the ego path along the lane as shown below. + +![occlusion_detection](./docs/occlusion_grid.drawio.svg) + +If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. + ### Module Parameters | Parameter | Type | Description | diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg new file mode 100644 index 0000000000000..fc09a4212070a --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg @@ -0,0 +1,2616 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + default stop line + +
+
+
+ + default stop line + + + + + + + + +
+
+
+ + occlusion peeking line + +
+
+
+ + occlusion peeking line + + + + + + + + + + + + + + + + + +
+
+
+ + ego + +
+
+
+ + ego + + + + + + + + +
+
+
+ occlusion attention area +
+
+
+ + occlusion attent... + + + + + + + + + + + +
+
+
+ + + stopped vehicle on attention area +
+ (blocking vehicle) + + +
+
+
+ + stopped vehicle on attentio... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + The cells behind blocking vehicles +
+ are not marked as occluded +
+ + +
+
+
+ + The cells behind blocking vehicles... + + + + + + + + +
+
+
+ + + mark the cells which is unknown and +
+ belong to attention area are +
+ marked as occluded + +
+ +
+
+
+ + mark the cells which is unknown and... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 13 +
+
+
+ + 13 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 12 +
+
+
+ + 12 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 11 +
+
+
+ + 11 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 10 +
+
+
+ + 10 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 9 +
+
+
+ + 9 + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 8 +
+
+
+ + 8 + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 7 +
+
+
+ + 7 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 6 +
+
+
+ + 6 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 5 +
+
+
+ + 5 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 4 +
+
+
+ + 4 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 4 +
+
+ +
+
+
+ + 4... + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 3 +
+
+
+ + 3 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 2 +
+
+
+ + 2 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 1 +
+
+
+ + 1 + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + nearest occlusion cell + +
+
+
+ + nearest occlusion... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index af8972424f54a..fbb82d6ace175 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -230,6 +230,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, + 0.99, 0.6), + &debug_marker_array, now); + /* appendMarkerArray( createPoseMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index ca1cbfdd0ccc7..89bb65f403095 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -929,28 +929,29 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!occlusion_attention_divisions_) { occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution / std::sqrt(2.0)); + planner_data_->occupancy_grid->info.resolution); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector parked_attention_objects; + std::vector blocking_attention_objects; std::copy_if( target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(parked_attention_objects), + std::back_inserter(blocking_attention_objects), [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { return std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; }); + debug_data_.blocking_attention_objects.objects = blocking_attention_objects; const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - parked_attention_objects, occlusion_dist_thr) + blocking_attention_objects, occlusion_dist_thr) : true; occlusion_stop_state_machine_.setStateWithMarginTime( is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, @@ -1064,9 +1065,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin); + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); if (is_in_adjacent_lanelets) { continue; } @@ -1078,17 +1081,19 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT if (is_in_intersection_area) { target_objects.objects.push_back(object); } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { target_objects.objects.push_back(object); } } else if (util::checkAngleForTargetLanelets( - object_direction, attention_area_lanelets, - planner_param_.common.attention_area_angle_thr, + object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, + attention_area_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin)) { + planner_param_.common.attention_area_margin, + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { // intersection_area is not available, use detection_area_with_margin as before target_objects.objects.push_back(object); } @@ -1240,7 +1245,7 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - [[maybe_unused]] const std::vector & + const std::vector & blocking_attention_objects, const double occlusion_dist_thr) { @@ -1265,6 +1270,13 @@ bool IntersectionModule::isOcclusionCleared( const int height = occ_grid.info.height; const double resolution = occ_grid.info.resolution; const auto & origin = occ_grid.info.origin.position; + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); @@ -1351,8 +1363,39 @@ bool IntersectionModule::isOcclusionCleared( cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); // (3) occlusion mask + static constexpr unsigned char OCCLUDED = 255; + static constexpr unsigned char BLOCKED = 127; cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // re-use attention_mask + attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + std::vector> blocking_polygons; + for (const auto & blocking_attention_object : blocking_attention_objects) { + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + findCommonCvPolygons(obj_poly.outer(), blocking_polygons); + } + for (const auto & blocking_polygon : blocking_polygons) { + cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); + } + for (const auto & lane_division : lane_divisions) { + const auto & divisions = lane_division.divisions; + for (const auto & division : divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + } + } + } + } // (4) extract occlusion polygons const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; @@ -1397,19 +1440,11 @@ bool IntersectionModule::isOcclusionCleared( } // (4.1) re-draw occluded cells using valid_contours occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); - for (unsigned i = 0; i < valid_contours.size(); ++i) { + for (const auto & valid_contour : valid_contours) { // NOTE: drawContour does not work well - cv::fillPoly(occlusion_mask, valid_contours[i], cv::Scalar(255), cv::LINE_AA); + cv::fillPoly(occlusion_mask, valid_contour, cv::Scalar(OCCLUDED), cv::LINE_AA); } - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - // (5) find distance // (5.1) discretize path_ip with resolution for computational cost LineString2d path_linestring; @@ -1496,7 +1531,11 @@ bool IntersectionModule::isOcclusionCleared( const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); // TODO(Mamoru Sobue): add handling for blocking vehicles if (!valid) continue; - if (occlusion_mask.at(height - 1 - idx_y, idx_x) == 255) { + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; + } + if (pixel == OCCLUDED) { if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index fff1d08223e5f..37dc83cbdf761 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1058,17 +1058,18 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( } bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin) + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold) { for (const auto & ll : target_lanelets) { - if (!lanelet::utils::isInLanelet(pose, ll, margin)) { + if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } const double ll_angle = lanelet::utils::getLaneletAngle(ll, pose.position); const double pose_angle = tf2::getYaw(pose.orientation); - const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle); + const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { return true; @@ -1077,6 +1078,14 @@ bool checkAngleForTargetLanelets( if (std::fabs(angle_diff) < detection_area_angle_thr) { return true; } + // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is + // positive + if ( + std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && + (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return true; + } } } return false; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index f02362e3f803d..7ba894bd9d32a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -136,9 +136,10 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, - const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, - const double margin = 0.0); + const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, + const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, + const bool consider_wrong_direction_vehicle, const double dist_margin, + const double parked_vehicle_speed_threshold); void cutPredictPathWithDuration( autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 4135884f03c74..7e5bff89485ed 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -50,6 +50,7 @@ struct DebugData std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; + autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; }; struct InterpolatedPathInfo From 89404e6070a43526f86a0bc88b231865ce2344af Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Mon, 2 Oct 2023 14:24:46 +0800 Subject: [PATCH 008/206] perf(system_monitor): fix program command line reading (#5191) * Fix program command line reading Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix spelling commandline->command_line Signed-off-by: Owen-Liuyuxuan --------- Signed-off-by: Owen-Liuyuxuan Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../process_monitor/process_monitor.hpp | 8 +++++++ .../src/process_monitor/process_monitor.cpp | 24 +++++++++++++++++-- 2 files changed, 30 insertions(+), 2 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index e114d58770883..42bd2a3ea9236 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -88,6 +88,14 @@ class ProcessMonitor : public rclcpp::Node */ void getHighMemoryProcesses(const std::string & output); + /** + * @brief get command line from process id + * @param [in] pid process id + * @param [out] command output command line + * @return true if success to get command line name + */ + bool getCommandLineFromPiD(const std::string & pid, std::string * command); + /** * @brief get top-rated processes * @param [in] tasks list of diagnostics tasks for high load procs diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f8ee314e5a0ef..6f58339f2ff4b 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,6 +414,19 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string * command) +{ + std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; + std::ifstream commandFile(commandLineFilePath); + if (commandFile.is_open()) { + std::getline(commandFile, *command); + commandFile.close(); + return true; + } else { + return false; + } +} + void ProcessMonitor::getTopratedProcesses( std::vector> * tasks, bp::pipe * p) { @@ -462,7 +475,14 @@ void ProcessMonitor::getTopratedProcesses( info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> info.cpuUsage >> info.memoryUsage >> info.cpuTime; - std::getline(stream, info.commandName); + std::string program_name; + std::getline(stream, program_name); + + bool flag_find_command_line = getCommandLineFromPiD(info.processId, &info.commandName); + + if (!flag_find_command_line) { + info.commandName = program_name; // if command line is not found, use program name instead + } tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); tasks->at(index)->setProcessInformation(info); @@ -515,7 +535,7 @@ void ProcessMonitor::onTimer() std::ostringstream os; // Get processes - bp::child c("top -bcn1 -o %CPU -w 256", bp::std_out > is_out, bp::std_err > is_err); + bp::child c("top -bn1 -o %CPU -w 128", bp::std_out > is_out, bp::std_err > is_err); c.wait(); if (c.exit_code() != 0) { From c6b802fc2908592cb4f27ae12feb6ca42015a51e Mon Sep 17 00:00:00 2001 From: beyzanurkaya <32412808+beyzanurkaya@users.noreply.github.com> Date: Mon, 2 Oct 2023 11:29:52 +0300 Subject: [PATCH 009/206] fix(behavior_path_planner): if ego leaves the current lane turn the signal on (#4348) * if ego leaves its lane due the avoidance, turn the signal on Signed-off-by: beyza * add right_bound check Signed-off-by: beyza * style(pre-commit): autofix --------- Signed-off-by: beyza Co-authored-by: beyza Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../avoidance/avoidance_module.cpp | 27 +++++++++++++++++-- 1 file changed, 25 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 83789818c456b..9117efcc447da 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" @@ -2543,9 +2544,31 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } } else { - turn_signal_info.turn_signal.command = TurnIndicatorsCommand::DISABLE; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const auto local_vehicle_footprint = + createVehicleFootprint(planner_data_->parameters.vehicle_info); + boost::geometry::model::ring shifted_vehicle_footprint; + for (const auto & cl : current_lanes) { + // get left and right bounds of current lane + const auto lane_left_bound = cl.leftBound2d().basicLineString(); + const auto lane_right_bound = cl.rightBound2d().basicLineString(); + for (size_t i = start_idx; i < end_idx; ++i) { + // transform vehicle footprint onto path points + shifted_vehicle_footprint = transformVector( + local_vehicle_footprint, + tier4_autoware_utils::pose2transform(path.path.points.at(i).point.pose)); + if ( + boost::geometry::intersects(lane_left_bound, shifted_vehicle_footprint) || + boost::geometry::intersects(lane_right_bound, shifted_vehicle_footprint)) { + if (segment_shift_length > 0.0) { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else { + turn_signal_info.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; + } + } + } + } } - if (ego_front_to_shift_start > 0.0) { turn_signal_info.desired_start_point = planner_data_->self_odometry->pose.pose; } else { From de07fb8011cb9330b435e13dadac74c875f9bae2 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Mon, 2 Oct 2023 12:04:32 +0300 Subject: [PATCH 010/206] fix(lidar_apollo_instance_segmentation): add arg data_path, update model path (#5139) Signed-off-by: Alexey Panferov --- .../launch/lidar_apollo_instance_segmentation.launch.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml index bceb928dd6692..7f5024929b531 100644 --- a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -7,7 +7,8 @@ - + + From bab14534a60e5741bd57b70979035bdc874c65cb Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 2 Oct 2023 21:01:09 +0900 Subject: [PATCH 011/206] feat(logger_level_reconfigure_plugin): use node interface and some cosmetic updates (#5204) * use node service Signed-off-by: Takamasa Horibe * enable yaml configuration Signed-off-by: Takamasa Horibe * update yaml loading Signed-off-by: Takamasa Horibe * make it scrollable Signed-off-by: Takamasa Horibe * change function order Signed-off-by: Takamasa Horibe * change color for level Signed-off-by: Takamasa Horibe * fix depend Signed-off-by: Takamasa Horibe * Update common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp Co-authored-by: Kosuke Takeuchi * Update common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp --------- Signed-off-by: Takamasa Horibe Co-authored-by: Kosuke Takeuchi --- .../CMakeLists.txt | 1 + .../config/logger_config.yaml | 59 +++++ .../logging_level_configure.hpp | 14 +- .../package.xml | 1 + .../src/logging_level_configure.cpp | 230 ++++++++---------- 5 files changed, 164 insertions(+), 141 deletions(-) create mode 100644 common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt index f86b6d4d2efdc..cc7da7e322d19 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt +++ b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt @@ -24,4 +24,5 @@ pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description. ament_auto_package( INSTALL_TO_SHARE plugins + config ) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml new file mode 100644 index 0000000000000..da5cc757682c5 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -0,0 +1,59 @@ +# logger_config.yaml + +# ============================================================ +# planning +# ============================================================ + +behavior_path_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils + +behavior_path_planner_avoidance: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance + +behavior_velocity_planner: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + +behavior_velocity_planner_intersection: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner.intersection + +motion_obstacle_avoidance: + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + +motion_velocity_smoother: + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: planning.scenario_planning.motion_velocity_smoother + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils + +# ============================================================ +# control +# ============================================================ + +lateral_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.lateral_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +longitudinal_controller: + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + +vehicle_cmd_gate: + - node_name: /control/vehicle_cmd_gate + logger_name: control.vehicle_cmd_gate + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp index f513a7e04a248..4d9b81ffb86bf 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp +++ b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp @@ -48,24 +48,24 @@ class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel QMap buttonGroups_; rclcpp::Node::SharedPtr raw_node_; - // logger_node_map_[target_name] = {container_name, logger_name} - std::map>> logger_node_map_; + // node_logger_map_[button_name] = {node_name, logger_name} + std::map>> node_logger_map_; - // client_map_[container_name] = service_client + // client_map_[node_name] = service_client std::unordered_map::SharedPtr> client_map_; - // button_map_[target_name][logging_level] = Q_button_pointer + // button_map_[button_name][logging_level] = Q_button_pointer std::unordered_map> button_map_; - QStringList getContainerList(); + QStringList getNodeList(); int getMaxModuleNameWidth(QLabel * containerLabel); void setLoggerNodeMap(); - void attachLoggingComponent(); private Q_SLOTS: void onButtonClick(QPushButton * button, const QString & name, const QString & level); - void updateButtonColors(const QString & target_module_name, QPushButton * active_button); + void updateButtonColors( + const QString & target_module_name, QPushButton * active_button, const QString & level); void changeLogLevel(const QString & container, const QString & level); }; diff --git a/common/tier4_logging_level_configure_rviz_plugin/package.xml b/common/tier4_logging_level_configure_rviz_plugin/package.xml index 4ba5bb1579e13..7849e6049a077 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/package.xml +++ b/common/tier4_logging_level_configure_rviz_plugin/package.xml @@ -21,6 +21,7 @@ rviz_common rviz_default_plugins rviz_rendering + yaml-cpp ament_lint_auto autoware_lint_common diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp index e645ff682d50e..b15c0f0f735fa 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp +++ b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp @@ -12,7 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "yaml-cpp/yaml.h" + #include +#include +#include #include #include @@ -26,59 +30,28 @@ LoggingLevelConfigureRvizPlugin::LoggingLevelConfigureRvizPlugin(QWidget * paren { } -// Calculate the maximum width among all target_module_name. -int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * containerLabel) -{ - int max_width = 0; - QFontMetrics metrics(containerLabel->font()); - for (const auto & item : logger_node_map_) { - const auto & target_module_name = item.first; - int width = metrics.horizontalAdvance(target_module_name); - if (width > max_width) { - max_width = width; - } - } - return max_width; -} - -// create container list in logger_node_map_ without -QStringList LoggingLevelConfigureRvizPlugin::getContainerList() -{ - QStringList containers; - for (const auto & item : logger_node_map_) { - const auto & container_logger_vec = item.second; - for (const auto & container_logger_pair : container_logger_vec) { - if (!containers.contains(container_logger_pair.first)) { - containers.append(container_logger_pair.first); - } - } - } - return containers; -} - void LoggingLevelConfigureRvizPlugin::onInitialize() { - setLoggerNodeMap(); + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - attachLoggingComponent(); + setLoggerNodeMap(); QVBoxLayout * layout = new QVBoxLayout; QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; - QStringList loaded_container; constexpr int height = 20; - for (const auto & item : logger_node_map_) { - const auto & target_module_name = item.first; + for (const auto & item : node_logger_map_) { + const auto & target_node_name = item.first; QHBoxLayout * hLayout = new QHBoxLayout; - // Create a QLabel to display the container name. - QLabel * containerLabel = new QLabel(target_module_name); - containerLabel->setFixedHeight(height); // Set fixed height for the button - containerLabel->setFixedWidth(getMaxModuleNameWidth(containerLabel)); + // Create a QLabel to display the node name. + QLabel * label = new QLabel(target_node_name); + label->setFixedHeight(height); // Set fixed height for the button + label->setFixedWidth(getMaxModuleNameWidth(label)); - hLayout->addWidget(containerLabel); // Add the QLabel to the hLayout. + hLayout->addWidget(label); // Add the QLabel to the hLayout. QButtonGroup * group = new QButtonGroup(this); for (const QString & level : levels) { @@ -86,42 +59,66 @@ void LoggingLevelConfigureRvizPlugin::onInitialize() btn->setFixedHeight(height); // Set fixed height for the button hLayout->addWidget(btn); // Add each QPushButton to the hLayout. group->addButton(btn); - button_map_[target_module_name][level] = btn; - connect(btn, &QPushButton::clicked, this, [this, btn, target_module_name, level]() { - this->onButtonClick(btn, target_module_name, level); + button_map_[target_node_name][level] = btn; + connect(btn, &QPushButton::clicked, this, [this, btn, target_node_name, level]() { + this->onButtonClick(btn, target_node_name, level); }); } // Set the "INFO" button as checked by default and change its color. - updateButtonColors(target_module_name, button_map_[target_module_name]["INFO"]); + updateButtonColors(target_node_name, button_map_[target_node_name]["INFO"], "INFO"); - buttonGroups_[target_module_name] = group; + buttonGroups_[target_node_name] = group; layout->addLayout(hLayout); } - setLayout(layout); + // Create a QWidget to hold the layout. + QWidget * containerWidget = new QWidget; + containerWidget->setLayout(layout); + + // Create a QScrollArea to make the layout scrollable. + QScrollArea * scrollArea = new QScrollArea; + scrollArea->setWidget(containerWidget); + scrollArea->setWidgetResizable(true); + + // Set the QScrollArea as the layout of the main widget. + QVBoxLayout * mainLayout = new QVBoxLayout; + mainLayout->addWidget(scrollArea); + setLayout(mainLayout); // set up service clients - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - const auto & containers = getContainerList(); - for (const QString & container : containers) { + const auto & nodes = getNodeList(); + for (const QString & node : nodes) { const auto client = raw_node_->create_client( - container.toStdString() + "/config_logger"); - client_map_[container] = client; + node.toStdString() + "/config_logger"); + client_map_[node] = client; } } -void LoggingLevelConfigureRvizPlugin::attachLoggingComponent() +// Calculate the maximum width among all target_module_name. +int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * label) { - const auto & containers = getContainerList(); - for (const auto & container_name : containers) { - // Load the component for each container - QString command = "ros2 component load --node-namespace " + container_name + - " --node-name logging_configure " + container_name + - " logging_demo logging_demo::LoggerConfig"; - int result = system(qPrintable(command)); - std::cerr << "load logger in " << container_name.toStdString() << ": result = " << result - << std::endl; + int max_width = 0; + QFontMetrics metrics(label->font()); + for (const auto & item : node_logger_map_) { + const auto & target_module_name = item.first; + max_width = std::max(metrics.horizontalAdvance(target_module_name), max_width); } + return max_width; +} + +// create node list in node_logger_map_ without +QStringList LoggingLevelConfigureRvizPlugin::getNodeList() +{ + QStringList nodes; + for (const auto & item : node_logger_map_) { + const auto & node_logger_vec = item.second; + for (const auto & node_logger_pair : node_logger_vec) { + if (!nodes.contains(node_logger_pair.first)) { + nodes.append(node_logger_pair.first); + } + } + } + return nodes; } // Modify the signature of the onButtonClick function: @@ -135,40 +132,48 @@ void LoggingLevelConfigureRvizPlugin::onButtonClick( << std::string(future.get()->success ? "success!" : "failed...") << std::endl; }; - for (const auto & container_logger_map : logger_node_map_[target_module_name]) { - const auto container_node = container_logger_map.first; - const auto logger_name = container_logger_map.second; + for (const auto & node_logger_map : node_logger_map_[target_module_name]) { + const auto node_name = node_logger_map.first; + const auto logger_name = node_logger_map.second; const auto req = std::make_shared(); req->logger_name = logger_name.toStdString(); req->level = level.toStdString(); std::cerr << "logger level of " << req->logger_name << " is set to " << req->level << std::endl; - client_map_[container_node]->async_send_request(req, callback); + client_map_[node_name]->async_send_request(req, callback); } updateButtonColors( - target_module_name, button); // Modify updateButtonColors to accept QPushButton only. + target_module_name, button, level); // Modify updateButtonColors to accept QPushButton only. } } void LoggingLevelConfigureRvizPlugin::updateButtonColors( - const QString & target_module_name, QPushButton * active_button) + const QString & target_module_name, QPushButton * active_button, const QString & level) { - const QString LIGHT_GREEN = "rgb(181, 255, 20)"; - const QString LIGHT_GRAY = "rgb(211, 211, 211)"; + std::unordered_map colorMap = { + {"DEBUG", "rgb(181, 255, 20)"}, /* green */ + {"INFO", "rgb(200, 255, 255)"}, /* light blue */ + {"WARN", "rgb(255, 255, 0)"}, /* yellow */ + {"ERROR", "rgb(255, 0, 0)"}, /* red */ + {"FATAL", "rgb(139, 0, 0)"}, /* dark red */ + {"OFF", "rgb(211, 211, 211)"} /* gray */ + }; + const QString LIGHT_GRAY_TEXT = "rgb(180, 180, 180)"; + const QString color = colorMap.count(level) ? colorMap[level] : colorMap["OFF"]; + for (const auto & button : button_map_[target_module_name]) { if (button.second == active_button) { - button.second->setStyleSheet("background-color: " + LIGHT_GREEN + "; color: black"); + button.second->setStyleSheet("background-color: " + color + "; color: black"); } else { button.second->setStyleSheet( - "background-color: " + LIGHT_GRAY + "; color: " + LIGHT_GRAY_TEXT); + "background-color: " + colorMap["OFF"] + "; color: " + LIGHT_GRAY_TEXT); } } } - void LoggingLevelConfigureRvizPlugin::save(rviz_common::Config config) const { Panel::save(config); @@ -181,68 +186,25 @@ void LoggingLevelConfigureRvizPlugin::load(const rviz_common::Config & config) void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() { - // =============================================================================================== - // ====================================== Planning =============================================== - // =============================================================================================== - - QString behavior_planning_container = - "/planning/scenario_planning/lane_driving/behavior_planning/behavior_planning_container"; - QString motion_planning_container = - "/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container"; - - // behavior_path_planner (all) - logger_node_map_["behavior_path_planner"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner"}, - {behavior_planning_container, "tier4_autoware_utils"}}; - - // behavior_path_planner: avoidance - logger_node_map_["behavior_path_planner: avoidance"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner." - "avoidance"}}; - - // behavior_velocity_planner (all) - logger_node_map_["behavior_velocity_planner"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner"}, - {behavior_planning_container, "tier4_autoware_utils"}}; - - // behavior_velocity_planner: intersection - logger_node_map_["behavior_velocity_planner: intersection"] = { - {behavior_planning_container, - "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner." - "intersection"}}; - - // obstacle_avoidance_planner - logger_node_map_["motion: obstacle_avoidance"] = { - {motion_planning_container, - "planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner"}, - {motion_planning_container, "tier4_autoware_utils"}}; - - // motion_velocity_smoother - QString container = "/planning/scenario_planning/motion_velocity_smoother_container"; - logger_node_map_["motion: velocity_smoother"] = { - {container, "planning.scenario_planning.motion_velocity_smoother"}, - {container, "tier4_autoware_utils"}}; - - // =============================================================================================== - // ======================================= Control =============================================== - // =============================================================================================== - - QString control_container = "/control/control_container"; - - // lateral_controller - logger_node_map_["lateral_controller"] = { - {control_container, "control.trajectory_follower.controller_node_exe.lateral_controller"}, - {control_container, "tier4_autoware_utils"}, - }; - - // longitudinal_controller - logger_node_map_["longitudinal_controller"] = { - {control_container, "control.trajectory_follower.controller_node_exe.longitudinal_controller"}, - {control_container, "tier4_autoware_utils"}, - }; + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("tier4_logging_level_configure_rviz_plugin"); + const std::string default_config_path = package_share_directory + "/config/logger_config.yaml"; + + const auto filename = + raw_node_->declare_parameter("config_filename", default_config_path); + RCLCPP_INFO(raw_node_->get_logger(), "load config file: %s", filename.c_str()); + + YAML::Node config = YAML::LoadFile(filename); + + for (YAML::const_iterator it = config.begin(); it != config.end(); ++it) { + const auto key = QString::fromStdString(it->first.as()); + const YAML::Node values = it->second; + for (size_t i = 0; i < values.size(); i++) { + const auto node_name = QString::fromStdString(values[i]["node_name"].as()); + const auto logger_name = QString::fromStdString(values[i]["logger_name"].as()); + node_logger_map_[key].push_back({node_name, logger_name}); + } + } } } // namespace rviz_plugin From e7bc187a9f4f86cbb7712e5fa9086a70d5b4652f Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 3 Oct 2023 06:57:08 +0900 Subject: [PATCH 012/206] fix(map_based_prediction): filter by distance for opposite lanes as well (#5195) * fix(map_based_prediction): filter by distance for opposite lanes as well Signed-off-by: kminoda * update Signed-off-by: kminoda --------- Signed-off-by: kminoda --- .../map_based_prediction/map_based_prediction_node.hpp | 3 +-- .../map_based_prediction/src/map_based_prediction_node.cpp | 7 +++---- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 8ef6d628aa2f6..3d01ab96e9b62 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -176,8 +176,7 @@ class MapBasedPredictionNode : public rclcpp::Node LaneletsData getCurrentLanelets(const TrackedObject & object); bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance = true); + const std::pair & lanelet, const TrackedObject & object); float calculateLocalLikelihood( const lanelet::Lanelet & current_lanelet, const TrackedObject & object) const; void updateObjectData(TrackedObject & object); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 6a414c26b8806..58356f4f96ed6 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -1251,7 +1251,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob for (const auto & lanelet : surrounding_opposite_lanelets) { // Check if the close lanelets meet the necessary condition for start lanelets // except for distance checking - if (!checkCloseLaneletCondition(lanelet, object, false)) { + if (!checkCloseLaneletCondition(lanelet, object)) { continue; } @@ -1271,8 +1271,7 @@ LaneletsData MapBasedPredictionNode::getCurrentLanelets(const TrackedObject & ob } bool MapBasedPredictionNode::checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const bool check_distance) + const std::pair & lanelet, const TrackedObject & object) { // Step1. If we only have one point in the centerline, we will ignore the lanelet if (lanelet.second.centerline().size() <= 1) { @@ -1307,7 +1306,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; const bool is_yaw_reversed = M_PI - delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta && object_vel < 0.0; - if (check_distance && dist_threshold_for_searching_lanelet_ < lanelet.first) { + if (dist_threshold_for_searching_lanelet_ < lanelet.first) { return false; } if (!is_yaw_reversed && delta_yaw_threshold_for_searching_lanelet_ < abs_norm_delta) { From a6af44eaf534a3c96e4bdb3b483f3eb99548027a Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 3 Oct 2023 06:57:21 +0900 Subject: [PATCH 013/206] feat(localization_error_monitor): update parameter (#5201) Signed-off-by: kminoda --- .../config/localization_error_monitor.param.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml index 2aa28014ead41..def5c80164642 100644 --- a/localization/localization_error_monitor/config/localization_error_monitor.param.yaml +++ b/localization/localization_error_monitor/config/localization_error_monitor.param.yaml @@ -1,7 +1,7 @@ /**: ros__parameters: scale: 3.0 - error_ellipse_size: 1.0 - warn_ellipse_size: 0.8 + error_ellipse_size: 1.5 + warn_ellipse_size: 1.2 error_ellipse_size_lateral_direction: 0.3 warn_ellipse_size_lateral_direction: 0.25 From ccf9235e7a5537ed83aea451216ac963487cbd36 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 3 Oct 2023 08:35:39 +0900 Subject: [PATCH 014/206] fix(avoidance): add debug info (#5205) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/avoidance/avoidance_module.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 9117efcc447da..36908bd87de23 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2662,6 +2662,7 @@ void AvoidanceModule::updateDebugMarker( addObjects(data.other_objects, std::string("OutOfTargetArea")); addObjects(data.other_objects, std::string("NotNeedAvoidance")); addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + addObjects(data.other_objects, std::string("TooNearToGoal")); } // shift line pre-process From 6e49e8297c915a0c68aaae7663d1ad89d4863221 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 3 Oct 2023 11:05:22 +0900 Subject: [PATCH 015/206] fix(simulator, controller): fix inverse pitch calculation (#5199) Signed-off-by: Mamoru Sobue Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe --- control/pid_longitudinal_controller/README.md | 4 + .../media/slope_definition.drawio.svg | 165 +++++++++ .../src/pid_longitudinal_controller.cpp | 4 +- simulator/simple_planning_simulator/README.md | 6 + .../media/pitch-calculation.drawio.svg | 329 ++++++++++++++++++ .../simple_planning_simulator_core.cpp | 2 +- 6 files changed, 508 insertions(+), 2 deletions(-) create mode 100644 control/pid_longitudinal_controller/media/slope_definition.drawio.svg create mode 100644 simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 418b3f7d27220..9d25322c793e0 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -75,6 +75,10 @@ For instance, if the vehicle is attempting to start with an acceleration of `1.0 A suitable example of a vehicle system for the slope compensation function is one in which the output acceleration from the longitudinal_controller is converted into target accel/brake pedal input without any feedbacks. In this case, the output acceleration is just used as a feedforward term to calculate the target pedal, and hence the issue mentioned above does not arise. +Note: The angle of the slope is defined as positive for an uphill slope, while the pitch angle of the ego pose is defined as negative when facing upward. They have an opposite definition. + +![slope_definition](./media/slope_definition.drawio.svg) + #### PID control For deviations that cannot be handled by FeedForward control, such as model errors, PID control is used to construct a feedback system. diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg new file mode 100644 index 0000000000000..7f0aa677629a3 --- /dev/null +++ b/control/pid_longitudinal_controller/media/slope_definition.drawio.svg @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Slope is defined as positive for an uphill +
+
+
+ + Slope is defined as positive for an uphi... + + + + + + + + + + + + + + + +
+
+
+ (Pitch of ego is defined as negative when facing upward) +
+
+
+ + (Pitch of ego is defined as negative when facing upwar... + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + +
+
+
+ y +
+
+
+ + z + + + + + + +
+
+
+ z +
+
+
+ + y + + + + + + +
+
+
+ d_z +
+
+
+ + d_z + + + + + + +
+
+
+ d_xy +
+
+
+ + d_xy + + + + + + + Text is not SVG - cannot display + + + diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 644effce50824..b80874a8a2e57 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -457,7 +457,9 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch - const double raw_pitch = longitudinal_utils::getPitchByPose(current_pose.orientation); + // NOTE: getPitchByTraj() calculates the pitch angle as defined in + // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed + const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation); const double traj_pitch = longitudinal_utils::getPitchByTraj(m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index d70aca794dc07..ceecacfe8cd8c 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -89,6 +89,12 @@ _Note_: The steering/velocity/acceleration dynamics is modeled by a first order Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. +### (Caveat) Pitch calculation + +Ego vehicle pitch angle is calculated in the following manner. + +![pitch calculation](./media/pitch-calculation.drawio.svg) + ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. diff --git a/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg new file mode 100644 index 0000000000000..d022b29d2a730 --- /dev/null +++ b/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg @@ -0,0 +1,329 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + forward
$diff_xy > 0
 + +
+
+
+ + forward... + + + + + +
+
+
+ + + ego +
+ direction +
+ + +
+
+
+ + ego... + + + + + +
+
+
+ + reverse
$diff_xy < 0
 + +
+
+
+ + reverse... + + + + + +
+
+
+ + + lane +
+ direction +
+ + +
+
+
+ + lane... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $diff_z > 0
 + +
+
+
+ + $diff_z > 0 + + + + + +
+
+
+ + $diff_z < 0
 + +
+
+
+ + $diff_z < 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + +
+
+
+ + pitch < 0
 + +
+
+
+ + pitch < 0 + + + + + +
+
+
+ + pitch > 0
 + +
+
+
+ + pitch > 0 + + + + + + + + Text is not SVG - cannot display + + + diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 8c2688928b4c9..208fd80d57a08 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -313,7 +313,7 @@ double SimplePlanningSimulator::calculate_ego_pitch() const std::cos(ego_yaw_against_lanelet); const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0; const double ego_pitch_angle = - reverse_sign ? std::atan2(-diff_z, -diff_xy) : std::atan2(diff_z, diff_xy); + reverse_sign ? -std::atan2(-diff_z, -diff_xy) : -std::atan2(diff_z, diff_xy); return ego_pitch_angle; } From 8ab910ba8dd3ff4fd09d149d6857b629b3cf9b0b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 3 Oct 2023 12:11:48 +0900 Subject: [PATCH 016/206] feat(crosswalk): dyanmic timeout for no intention to walk decision (#5188) * feat(crosswalk): dyanmic timeout for no intention to walk decision Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/crosswalk.param.yaml | 4 +- .../src/manager.cpp | 6 ++- .../src/scene_crosswalk.cpp | 2 +- .../src/scene_crosswalk.hpp | 54 ++++++++++++++++--- 4 files changed, 55 insertions(+), 11 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index b1e5bcb32bd18..e7accc14096e0 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -54,7 +54,9 @@ ## param for yielding disable_stop_for_yield_cancel: true # for the crosswalk where there is a traffic signal disable_yield_for_new_stopped_object: true # for the crosswalk where there is a traffic signal - timeout_no_intention_to_walk: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + # if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. + distance_map_for_no_intention_to_walk: [1.0, 5.0] # [m] ascending order + timeout_map_for_no_intention_to_walk: [3.0, 0.0] # [s] timeout_ego_stop_for_yield: 3.0 # [s] the amount of time which ego should be stopping to query whether it yields or not # param for target object filtering diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 304b4bdf6f5e7..94e87d0174193 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -104,8 +104,10 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".pass_judge.disable_stop_for_yield_cancel"); cp.disable_yield_for_new_stopped_object = getOrDeclareParameter(node, ns + ".pass_judge.disable_yield_for_new_stopped_object"); - cp.timeout_no_intention_to_walk = - getOrDeclareParameter(node, ns + ".pass_judge.timeout_no_intention_to_walk"); + cp.distance_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.distance_map_for_no_intention_to_walk"); + cp.timeout_map_for_no_intention_to_walk = getOrDeclareParameter>( + node, ns + ".pass_judge.timeout_map_for_no_intention_to_walk"); cp.timeout_ego_stop_for_yield = getOrDeclareParameter(node, ns + ".pass_judge.timeout_ego_stop_for_yield"); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index b99bd522b81f8..62acd191d8697 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -928,7 +928,7 @@ void CrosswalkModule::updateObjectState( getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, planner_param_); + has_traffic_light, collision_point, planner_param_, crosswalk_.polygon2d().basicPolygon()); if (collision_point) { const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 667298cbb4c58..063ea4f83cd45 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -30,12 +30,15 @@ #include #include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -45,6 +48,7 @@ namespace behavior_velocity_planner { +namespace bg = boost::geometry; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -71,6 +75,33 @@ double interpolateEgoPassMargin( } return y_vec.back(); } + +double InterpolateMap( + const std::vector & key_map, const std::vector & value_map, const double query) +{ + // If the speed is out of range of the key_map, apply zero-order hold. + if (query <= key_map.front()) { + return value_map.front(); + } + if (query >= key_map.back()) { + return value_map.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < key_map.size() - 1; ++i) { + if (key_map.at(i) <= query && query <= key_map.at(i + 1)) { + auto ratio = (query - key_map.at(i)) / std::max(key_map.at(i + 1) - key_map.at(i), 1.0e-5); + ratio = std::clamp(ratio, 0.0, 1.0); + const auto interp = value_map.at(i) + ratio * (value_map.at(i + 1) - value_map.at(i)); + return interp; + } + } + + std::cerr << "Crosswalk's InterpolateMap interpolation logic is broken." + "Please check the code." + << std::endl; + return value_map.back(); +} } // namespace class CrosswalkModule : public SceneModuleInterface @@ -111,7 +142,8 @@ class CrosswalkModule : public SceneModuleInterface double min_object_velocity; bool disable_stop_for_yield_cancel; bool disable_yield_for_new_stopped_object; - double timeout_no_intention_to_walk; + std::vector distance_map_for_no_intention_to_walk; + std::vector timeout_map_for_no_intention_to_walk; double timeout_ego_stop_for_yield; // param for input data double traffic_light_state_timeout; @@ -132,9 +164,10 @@ class CrosswalkModule : public SceneModuleInterface std::optional collision_point{}; void transitState( - const rclcpp::Time & now, const double vel, const bool is_ego_yielding, - const bool has_traffic_light, const std::optional & collision_point, - const PlannerParam & planner_param) + const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, + const bool is_ego_yielding, const bool has_traffic_light, + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { const bool is_stopped = vel < planner_param.stop_object_velocity; @@ -147,8 +180,13 @@ class CrosswalkModule : public SceneModuleInterface if (!time_to_start_stopped) { time_to_start_stopped = now; } + const double distance_to_crosswalk = + bg::distance(crosswalk_polygon, lanelet::BasicPoint2d(position.x, position.y)); + const double timeout_no_intention_to_walk = InterpolateMap( + planner_param.distance_map_for_no_intention_to_walk, + planner_param.timeout_map_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = - (now - *time_to_start_stopped).seconds() < planner_param.timeout_no_intention_to_walk; + (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; if ( (is_ego_yielding || (has_traffic_light && planner_param.disable_stop_for_yield_cancel)) && !intent_to_cross) { @@ -203,7 +241,8 @@ class CrosswalkModule : public SceneModuleInterface void update( const std::string & uuid, const geometry_msgs::msg::Point & position, const double vel, const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, - const std::optional & collision_point, const PlannerParam & planner_param) + const std::optional & collision_point, const PlannerParam & planner_param, + const lanelet::BasicPolygon2d & crosswalk_polygon) { // update current uuids current_uuids_.push_back(uuid); @@ -221,7 +260,8 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( - now, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param); + now, position, vel, is_ego_yielding, has_traffic_light, collision_point, planner_param, + crosswalk_polygon); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; } From 8c3812603173b68ec8fc3c5b7b300e170fa2bea6 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 3 Oct 2023 12:24:25 +0900 Subject: [PATCH 017/206] fix(out_of_lane): improve stop decision (#5207) Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 8 +- .../README.md | 15 ++-- .../config/out_of_lane.param.yaml | 3 +- .../src/calculate_slowdown_points.hpp | 87 ++++++++++++++++++ .../src/decisions.cpp | 20 ++--- .../src/filter_predicted_objects.hpp | 10 +-- .../src/footprint.cpp | 6 +- .../src/lanelets_selection.cpp | 3 + .../src/manager.cpp | 2 +- .../src/scene_out_of_lane.cpp | 90 +++++-------------- .../src/scene_out_of_lane.hpp | 9 -- .../src/types.hpp | 4 +- 12 files changed, 136 insertions(+), 121 deletions(-) create mode 100644 planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt index 633d5fdd613af..8ce1a334c4b35 100644 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt @@ -6,13 +6,7 @@ autoware_package() pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED - src/debug.cpp - src/decisions.cpp - src/footprint.cpp - src/lanelets_selection.cpp - src/manager.cpp - src/overlapping_range.cpp - src/scene_out_of_lane.cpp + DIRECTORY src ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md index 885c0dc2859ab..99396eb0edf22 100644 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ b/planning/behavior_velocity_out_of_lane_module/README.md @@ -149,14 +149,13 @@ Moreover, parameter `action.distance_buffer` adds an extra distance between the | `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | | `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `strict` | bool | [-] if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego; if false, ego stops just before entering a lane but may then be overlapping another lane | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m/s] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | +| Parameter /action | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | +| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | +| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | +| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | +| `slowdown.velocity` | double | [m] slow down velocity | +| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | | Parameter /ego | Type | Description | | -------------------- | ------ | ---------------------------------------------------- | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml index 510dc86ef651d..c74c5b3d87c1d 100644 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml @@ -24,8 +24,7 @@ action: # action to insert in the path if an object causes a conflict at an overlap skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - strict: true # if true, when a decision is taken to avoid entering a lane, the stop point will make sure no lane at all is entered by ego - # if false, ego stops just before entering a lane but may then be overlapping another lane. + precision: 0.1 # [m] precision when inserting a stop pose in the path distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane slowdown: distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp new file mode 100644 index 0000000000000..5d22f01222919 --- /dev/null +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -0,0 +1,87 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CALCULATE_SLOWDOWN_POINTS_HPP_ +#define CALCULATE_SLOWDOWN_POINTS_HPP_ + +#include "footprint.hpp" +#include "types.hpp" + +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner::out_of_lane +{ + +bool can_decelerate( + const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) +{ + if (ego_data.velocity < 0.5) return true; + const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( + ego_data.path.points, ego_data.pose.position, point.point.pose.position); + const auto acc_to_target_vel = + (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); + return acc_to_target_vel < std::abs(ego_data.max_decel); +} + +std::optional calculate_last_in_lane_pose( + const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, + const PlannerParam & params) +{ + const auto from_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); + const auto to_arc_length = + motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx); + PathPointWithLaneId interpolated_point; + for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { + // TODO(Maxime): binary search + interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); + const auto respect_decel_limit = + !params.skip_if_over_max_decel || + can_decelerate(ego_data, interpolated_point, decision.velocity); + if ( + respect_decel_limit && !boost::geometry::overlaps( + project_to_pose(footprint, interpolated_point.point.pose), + decision.lane_to_avoid.polygon2d().basicPolygon())) + return interpolated_point; + } + return std::nullopt; +} + +/// @brief calculate the slowdown point to insert in the path +/// @param ego_data ego data (path, velocity, etc) +/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) +/// @param params parameters +/// @return optional slowdown point to insert in the path +std::optional calculate_slowdown_point( + const EgoData & ego_data, const std::vector & decisions, PlannerParam params) +{ + params.extra_front_offset += params.dist_buffer; + const auto base_footprint = make_base_footprint(params); + + // search for the first slowdown decision for which a stop point can be inserted + for (const auto & decision : decisions) { + const auto last_in_lane_pose = + calculate_last_in_lane_pose(ego_data, decision, base_footprint, params); + if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; + } + return std::nullopt; +} +} // namespace behavior_velocity_planner::out_of_lane +#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index da46c1355e735..78d9b73c86a17 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -35,7 +35,7 @@ namespace behavior_velocity_planner::out_of_lane double distance_along_path(const EgoData & ego_data, const size_t target_idx) { return motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, ego_data.first_path_idx + target_idx); + ego_data.path.points, ego_data.pose.position, ego_data.first_path_idx + target_idx); } double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) @@ -43,7 +43,7 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx, const const auto dist = distance_along_path(ego_data, target_idx); const auto v = std::max( std::max(ego_data.velocity, min_velocity), - ego_data.path->points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * + ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * 0.5); return dist / v; } @@ -126,11 +126,9 @@ std::optional> object_time_to_range( const auto same_driving_direction_as_ego = enter_time < exit_time; if (same_driving_direction_as_ego) { - RCLCPP_DEBUG(logger, " / SAME DIR \\\n"); worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; } else { - RCLCPP_DEBUG(logger, " / OPPOSITE DIR \\\n"); worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; } @@ -212,8 +210,11 @@ std::optional> object_time_to_range( bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) { - return std::min(range_times.object.enter_time, range_times.object.exit_time) < - params.time_threshold; + const auto enter_within_threshold = + range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; + const auto exit_within_threshold = + range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; + return enter_within_threshold || exit_within_threshold; } bool intervals_condition( @@ -357,11 +358,10 @@ std::optional calculate_decision( { std::optional decision; if (should_not_enter(range, inputs, params, logger)) { - const auto stop_before_range = params.strict ? find_most_preceding_range(range, inputs) : range; decision.emplace(); - decision->target_path_idx = inputs.ego_data.first_path_idx + - stop_before_range.entering_path_idx; // add offset from curr pose - decision->lane_to_avoid = stop_before_range.lane; + decision->target_path_idx = + inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose + decision->lane_to_avoid = range.lane; const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx); set_decision_velocity(decision, ego_dist_to_range, params); } diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index f2555f31a4d5e..13bad0d047922 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -23,9 +23,6 @@ namespace behavior_velocity_planner::out_of_lane { -using motion_utils::calcLateralOffset; -using tier4_autoware_utils::calcDistance2d; - /// @brief filter predicted objects and their predicted paths /// @param [in] objects predicted objects to filter /// @param [in] ego_data ego data @@ -47,13 +44,8 @@ autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( auto filtered_object = object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); - - // Note: lateral offset can not be calculated for path with less than 2 unique points const auto lat_offset_to_current_ego = - unique_path_points.size() > 1 - ? std::abs(calcLateralOffset(unique_path_points, ego_data.pose.position)) - : calcDistance2d(unique_path_points.front(), ego_data.pose.position); + std::abs(motion_utils::calcLateralOffset(predicted_path.path, ego_data.pose.position)); const auto is_crossing_ego = lat_offset_to_current_ego <= object.shape.dimensions.y / 2.0 + std::max( diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index 9ad7437901309..31cc035703a7b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -57,9 +57,9 @@ std::vector calculate_path_footprints( { const auto base_footprint = make_base_footprint(params); std::vector path_footprints; - path_footprints.reserve(ego_data.path->points.size()); - for (auto i = ego_data.first_path_idx; i < ego_data.path->points.size(); ++i) { - const auto & path_pose = ego_data.path->points[i].point.pose; + path_footprints.reserve(ego_data.path.points.size()); + for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size(); ++i) { + const auto & path_pose = ego_data.path.points[i].point.pose; const auto angle = tf2::getYaw(path_pose.orientation); const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); lanelet::BasicPolygon2d footprint; diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 7f3b69f40840b..c5dedf23785ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace behavior_velocity_planner::out_of_lane { @@ -68,6 +69,8 @@ lanelet::ConstLanelets calculate_other_lanelets( return false; }; for (const auto & ll : lanelets_within_range) { + if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") + continue; const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); if (!is_path_lanelet && !is_ignored_lanelet && !is_overlapped_by_path_lanelets(ll.second)) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 5765363024aed..2422c6fe84e56 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -56,7 +56,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.skip_if_over_max_decel = getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); - pp.strict = getOrDeclareParameter(node, ns + ".action.strict"); + pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); pp.slow_dist_threshold = diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 013643b6bd6d4..1487d9e63c8b0 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -14,6 +14,7 @@ #include "scene_out_of_lane.hpp" +#include "calculate_slowdown_points.hpp" #include "debug.hpp" #include "decisions.hpp" #include "filter_predicted_objects.hpp" @@ -31,6 +32,8 @@ #include #include +#include + #include #include @@ -62,9 +65,9 @@ bool OutOfLaneModule::modifyPathVelocity( stopwatch.tic(); EgoData ego_data; ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path = path; + ego_data.path.points = path->points; ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); + motion_utils::findNearestSegmentIndex(ego_data.path.points, ego_data.pose.position); ego_data.velocity = planner_data_->current_velocity->twist.linear.x; ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; stopwatch.tic("calculate_path_footprints"); @@ -73,7 +76,7 @@ bool OutOfLaneModule::modifyPathVelocity( const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); // Calculate lanelets to ignore and consider const auto path_lanelets = planning_utils::getLaneletsOnPath( - *path, planner_data_->route_handler_->getLaneletMapPtr(), + ego_data.path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); const auto ignored_lanelets = calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); @@ -113,22 +116,27 @@ bool OutOfLaneModule::modifyPathVelocity( auto decisions = calculate_decisions(inputs, params_, logger_); const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); stopwatch.tic("calc_slowdown_points"); - const auto points_to_insert = calculate_slowdown_points(ego_data, decisions, params_); - debug_data_.slowdowns = points_to_insert; + const auto point_to_insert = calculate_slowdown_point(ego_data, decisions, params_); const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); stopwatch.tic("insert_slowdown_points"); - for (const auto & point : points_to_insert) { - auto path_idx = point.slowdown.target_path_idx; - planning_utils::insertVelocity(*ego_data.path, point.point, point.slowdown.velocity, path_idx); - if (point.slowdown.velocity == 0.0) { + debug_data_.slowdowns.clear(); + if (point_to_insert) { + debug_data_.slowdowns = {*point_to_insert}; + auto path_idx = motion_utils::findNearestSegmentIndex( + path->points, point_to_insert->point.point.pose.position) + + 1; + planning_utils::insertVelocity( + *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx, + params_.precision); + if (point_to_insert->slowdown.velocity == 0.0) { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = point.point.point.pose; + stop_factor.stop_pose = point_to_insert->point.point.pose; stop_factor.dist_to_stop_pose = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, point.point.point.pose.position); + ego_data.path.points, ego_data.pose.position, point_to_insert->point.point.pose.position); planning_utils::appendStopReason(stop_factor, stop_reason); } velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, point.point.point.pose, + path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, VelocityFactor::UNKNOWN); } const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); @@ -181,62 +189,4 @@ motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() return virtual_walls; } -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params) -{ - std::vector to_insert; - params.extra_front_offset += params.dist_buffer; - const auto base_footprint = make_base_footprint(params); - - const auto can_decel = [&](const auto dist_ahead_of_ego, const auto target_vel) { - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); - }; - const auto insert_decision = [&](const auto & path_point, const auto & decision) -> bool { - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( - ego_data.path->points, ego_data.pose.position, path_point.point.pose.position); - if (!params.skip_if_over_max_decel || can_decel(dist_ahead_of_ego, decision.velocity)) { - to_insert.push_back({decision, path_point}); - return true; - } - return false; - }; - const auto insert_interpolated_decision = - [&](const auto & path_point, const auto & decision) -> bool { - auto interpolated_point = path_point; - const auto & path_pose = path_point.point.pose; - const auto & prev_path_pose = ego_data.path->points[decision.target_path_idx - 1].point.pose; - constexpr auto precision = 0.1; - for (auto ratio = precision; ratio <= 1.0; ratio += precision) { - interpolated_point.point.pose = - tier4_autoware_utils::calcInterpolatedPose(path_pose, prev_path_pose, ratio, false); - const auto is_overlap = boost::geometry::overlaps( - project_to_pose(base_footprint, interpolated_point.point.pose), - decision.lane_to_avoid.polygon2d().basicPolygon()); - if (!is_overlap) { - return insert_decision(path_point, decision); - } - } - return false; - }; - for (const auto & decision : decisions) { - const auto & path_point = ego_data.path->points[decision.target_path_idx]; - const auto decision_is_at_beginning_of_path = - decision.target_path_idx == ego_data.first_path_idx; - bool inserted = false; - if (decision_is_at_beginning_of_path) { - inserted = insert_decision(path_point, decision); - } else { - inserted = insert_interpolated_decision(path_point, decision); - // if no valid point found, fallback to using the previous index (known to not overlap) - if (!inserted) - inserted = insert_decision(ego_data.path->points[decision.target_path_idx], decision); - } - // only insert the first (i.e., lowest arc length) decision - if (inserted) break; - } - return to_insert; -} - } // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 1a64c11a3c921..cdf8eef8f277b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -57,15 +57,6 @@ class OutOfLaneModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; - -/// @brief calculate points along the path where we want ego to slowdown/stop -/// @param ego_data ego data (path, velocity, etc) -/// @param decisions decisions (before which point to stop, what lane to avoid entering, etc) -/// @param params parameters -/// @return precise points to insert in the path -std::vector calculate_slowdown_points( - const EgoData & ego_data, const std::vector & decisions, PlannerParam params); - } // namespace behavior_velocity_planner::out_of_lane #endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index b81fa09884819..64d88f15e6966 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -52,11 +52,11 @@ struct PlannerParam double overlap_min_dist; // [m] min distance inside another lane to consider an overlap // action to insert in the path if an object causes a conflict at an overlap bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - bool strict; // if true stop before entering *any* other lane, not only the lane to avoid double dist_buffer; double slow_velocity; double slow_dist_threshold; double stop_dist_threshold; + double precision; // [m] precision when inserting a stop pose in the path // ego dimensions used to create its polygon footprint double front_offset; // [m] front offset (from vehicle info) double rear_offset; // [m] rear offset (from vehicle info) @@ -135,7 +135,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId * path{}; + autoware_auto_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] From 58a513efabcce9b93d1bc2ee4639d7b15b852038 Mon Sep 17 00:00:00 2001 From: shulanbushangshu <102840938+shulanbushangshu@users.noreply.github.com> Date: Tue, 3 Oct 2023 11:33:33 +0800 Subject: [PATCH 018/206] feat: add common interface specs test (#5034) * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * add interface specs test Signed-off-by: jack.song * style(pre-commit): autofix * fix copyright Signed-off-by: Takagi, Isamu --------- Signed-off-by: jack.song Signed-off-by: Takagi, Isamu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Takagi, Isamu --- .../component_interface_specs/CMakeLists.txt | 34 ++++++++++ common/component_interface_specs/package.xml | 17 ++++- .../test/gtest_main.cpp | 21 ++++++ .../test/test_control.cpp | 46 +++++++++++++ .../test/test_localization.cpp | 46 +++++++++++++ .../test/test_map.cpp | 28 ++++++++ .../test/test_perception.cpp | 28 ++++++++ .../test/test_planning.cpp | 64 +++++++++++++++++++ .../test/test_system.cpp | 37 +++++++++++ .../test/test_vehicle.cpp | 64 +++++++++++++++++++ 10 files changed, 384 insertions(+), 1 deletion(-) create mode 100644 common/component_interface_specs/test/gtest_main.cpp create mode 100644 common/component_interface_specs/test/test_control.cpp create mode 100644 common/component_interface_specs/test/test_localization.cpp create mode 100644 common/component_interface_specs/test/test_map.cpp create mode 100644 common/component_interface_specs/test/test_perception.cpp create mode 100644 common/component_interface_specs/test/test_planning.cpp create mode 100644 common/component_interface_specs/test/test_system.cpp create mode 100644 common/component_interface_specs/test/test_vehicle.cpp diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index 31c723600183f..b519a0ddce86a 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -4,4 +4,38 @@ project(component_interface_specs) find_package(autoware_cmake REQUIRED) autoware_package() +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} + ${autoware_auto_planning_msgs_INCLUDE_DIRS} + ${autoware_planning_msgs_INCLUDE_DIRS} + ${autoware_auto_vehicle_msgs_INCLUDE_DIRS} + ${tier4_control_msgs_INCLUDE_DIRS} + ${nav_msgs_INCLUDE_DIRS} + ${tier4_system_msgs_INCLUDE_DIRS} + ${tier4_vehicle_msgs_INCLUDE_DIRS} + ${autoware_auto_perception_msgs_INCLUDE_DIRS} + ${tier4_map_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_specs + test/gtest_main.cpp + test/test_planning.cpp + test/test_control.cpp + test/test_localization.cpp + test/test_system.cpp + test/test_map.cpp + test/test_perception.cpp + test/test_vehicle.cpp + ) + target_include_directories(test_component_interface_specs + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 7ebc3f0d1bea9..93f77c651869d 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -11,13 +11,28 @@ Apache License 2.0 ament_cmake_auto + ament_cmake_core + ament_cmake_export_dependencies + ament_cmake_test autoware_cmake + ament_cmake_core + ament_cmake_test + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_planning_msgs + nav_msgs + rcl + rclcpp + rosidl_runtime_cpp + tier4_control_msgs tier4_map_msgs + tier4_system_msgs tier4_vehicle_msgs - + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/component_interface_specs/test/gtest_main.cpp b/common/component_interface_specs/test/gtest_main.cpp new file mode 100644 index 0000000000000..81d9d5345270d --- /dev/null +++ b/common/component_interface_specs/test/gtest_main.cpp @@ -0,0 +1,21 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" + +int main(int argc, char * argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/common/component_interface_specs/test/test_control.cpp b/common/component_interface_specs/test/test_control.cpp new file mode 100644 index 0000000000000..366641eacbd23 --- /dev/null +++ b/common/component_interface_specs/test/test_control.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/control.hpp" +#include "gtest/gtest.h" + +TEST(control, interface) +{ + { + using control_interface::IsPaused; + IsPaused is_paused; + size_t depth = 1; + EXPECT_EQ(is_paused.depth, depth); + EXPECT_EQ(is_paused.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_paused.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStartRequested; + IsStartRequested is_start_requested; + size_t depth = 1; + EXPECT_EQ(is_start_requested.depth, depth); + EXPECT_EQ(is_start_requested.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_start_requested.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using control_interface::IsStopped; + IsStopped is_stopped; + size_t depth = 1; + EXPECT_EQ(is_stopped.depth, depth); + EXPECT_EQ(is_stopped.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(is_stopped.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_localization.cpp b/common/component_interface_specs/test/test_localization.cpp new file mode 100644 index 0000000000000..31d8e139bd69a --- /dev/null +++ b/common/component_interface_specs/test/test_localization.cpp @@ -0,0 +1,46 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/localization.hpp" +#include "gtest/gtest.h" + +TEST(localization, interface) +{ + { + using localization_interface::InitializationState; + InitializationState initialization_state; + size_t depth = 1; + EXPECT_EQ(initialization_state.depth, depth); + EXPECT_EQ(initialization_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(initialization_state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using localization_interface::KinematicState; + KinematicState kinematic_state; + size_t depth = 1; + EXPECT_EQ(kinematic_state.depth, depth); + EXPECT_EQ(kinematic_state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(kinematic_state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using localization_interface::Acceleration; + Acceleration acceleration; + size_t depth = 1; + EXPECT_EQ(acceleration.depth, depth); + EXPECT_EQ(acceleration.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(acceleration.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_map.cpp b/common/component_interface_specs/test/test_map.cpp new file mode 100644 index 0000000000000..a65f2cb7120d1 --- /dev/null +++ b/common/component_interface_specs/test/test_map.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/map.hpp" +#include "gtest/gtest.h" + +TEST(map, interface) +{ + { + using map_interface::MapProjectorInfo; + MapProjectorInfo map_projector; + size_t depth = 1; + EXPECT_EQ(map_projector.depth, depth); + EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_perception.cpp b/common/component_interface_specs/test/test_perception.cpp new file mode 100644 index 0000000000000..ec80c06bb119a --- /dev/null +++ b/common/component_interface_specs/test/test_perception.cpp @@ -0,0 +1,28 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/perception.hpp" +#include "gtest/gtest.h" + +TEST(perception, interface) +{ + { + using perception_interface::ObjectRecognition; + ObjectRecognition object_recognition; + size_t depth = 1; + EXPECT_EQ(object_recognition.depth, depth); + EXPECT_EQ(object_recognition.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(object_recognition.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_planning.cpp b/common/component_interface_specs/test/test_planning.cpp new file mode 100644 index 0000000000000..8c504cb119854 --- /dev/null +++ b/common/component_interface_specs/test/test_planning.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/planning.hpp" +#include "gtest/gtest.h" + +TEST(planning, interface) +{ + { + using planning_interface::RouteState; + RouteState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Route; + Route route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::NormalRoute; + NormalRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::MrmRoute; + MrmRoute route; + size_t depth = 1; + EXPECT_EQ(route.depth, depth); + EXPECT_EQ(route.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(route.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } + + { + using planning_interface::Trajectory; + Trajectory trajectory; + size_t depth = 1; + EXPECT_EQ(trajectory.depth, depth); + EXPECT_EQ(trajectory.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(trajectory.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} diff --git a/common/component_interface_specs/test/test_system.cpp b/common/component_interface_specs/test/test_system.cpp new file mode 100644 index 0000000000000..416d2effad766 --- /dev/null +++ b/common/component_interface_specs/test/test_system.cpp @@ -0,0 +1,37 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/system.hpp" +#include "gtest/gtest.h" + +TEST(system, interface) +{ + { + using system_interface::MrmState; + MrmState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using system_interface::OperationModeState; + OperationModeState state; + size_t depth = 1; + EXPECT_EQ(state.depth, depth); + EXPECT_EQ(state.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(state.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + } +} diff --git a/common/component_interface_specs/test/test_vehicle.cpp b/common/component_interface_specs/test/test_vehicle.cpp new file mode 100644 index 0000000000000..1f2041b6cb79e --- /dev/null +++ b/common/component_interface_specs/test/test_vehicle.cpp @@ -0,0 +1,64 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_specs/vehicle.hpp" +#include "gtest/gtest.h" + +TEST(vehicle, interface) +{ + { + using vehicle_interface::SteeringStatus; + SteeringStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::GearStatus; + GearStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::TurnIndicatorStatus; + TurnIndicatorStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::HazardLightStatus; + HazardLightStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } + + { + using vehicle_interface::EnergyStatus; + EnergyStatus status; + size_t depth = 1; + EXPECT_EQ(status.depth, depth); + EXPECT_EQ(status.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE); + EXPECT_EQ(status.durability, RMW_QOS_POLICY_DURABILITY_VOLATILE); + } +} From 81ede03f749d46fb3eed3c9e96ce9d993acaf8c3 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 3 Oct 2023 12:52:18 +0900 Subject: [PATCH 019/206] fix(utils): fix bug in drivable area expansion (#5198) * fix(avoidance): fix invalid drivable area bound Signed-off-by: satoshi-ota * refactor(avoidance): split huge process to three functions Signed-off-by: satoshi-ota * fix(utils): improve sharp bound remove logic Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner/utils/utils.hpp | 9 + .../behavior_path_planner/src/utils/utils.cpp | 347 +++++++++++------- 2 files changed, 230 insertions(+), 126 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 3d81c584b7c01..c5e5aa3e50191 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -231,6 +231,15 @@ std::vector calcBound( const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool is_left); +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler); + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left); + boost::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 1b14741db4a56..7305405b50410 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1552,31 +1552,10 @@ void generateDrivableArea( } } -// calculate bounds from drivable lanes and hatched road markings -std::vector calcBound( - const std::shared_ptr route_handler, - const std::vector & drivable_lanes, - const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, - const bool is_left) +std::vector getBoundWithHatchedRoadMarkings( + const std::vector & original_bound, + const std::shared_ptr & route_handler) { - // a function to convert drivable lanes to points without duplicated points - const auto convert_to_points = [&](const std::vector & drivable_lanes) { - constexpr double overlap_threshold = 0.01; - - std::vector points; - for (const auto & drivable_lane : drivable_lanes) { - const auto bound = - is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); - for (const auto & point : bound) { - if ( - points.empty() || - overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { - points.push_back(point); - } - } - } - return points; - }; // a function to get polygon with a designated point id const auto get_corresponding_polygon_index = [&](const auto & polygon, const auto & target_point_id) { @@ -1589,18 +1568,89 @@ std::vector calcBound( // This means calculation has some errors. return polygon.size() - 1; }; + const auto mod = [&](const int a, const int b) { return (a + b) % b; // NOTE: consider negative value }; + + std::vector expanded_bound{}; + + std::optional current_polygon{std::nullopt}; + std::vector current_polygon_border_indices; + // expand drivable area by hatched road markings. + for (size_t bound_point_idx = 0; bound_point_idx < original_bound.size(); ++bound_point_idx) { + const auto & bound_point = original_bound[bound_point_idx]; + const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); + + bool will_close_polygon{false}; + if (!current_polygon) { + if (!polygon) { + expanded_bound.push_back(bound_point); + } else { + // There is a new additional polygon to expand + current_polygon = polygon; + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } else { + if (!polygon) { + will_close_polygon = true; + } else { + current_polygon_border_indices.push_back( + get_corresponding_polygon_index(*current_polygon, bound_point.id())); + } + } + + if (bound_point_idx == original_bound.size() - 1 && current_polygon) { + // If drivable lanes ends earlier than polygon, close the polygon + will_close_polygon = true; + } + + if (will_close_polygon) { + // The current additional polygon ends to expand + if (current_polygon_border_indices.size() == 1) { + expanded_bound.push_back((*current_polygon)[current_polygon_border_indices.front()]); + } else { + const size_t current_polygon_points_num = current_polygon->size(); + const bool is_polygon_opposite_direction = [&]() { + const size_t modulo_diff = mod( + static_cast(current_polygon_border_indices[1]) - + static_cast(current_polygon_border_indices[0]), + current_polygon_points_num); + return modulo_diff == 1; + }(); + + const int target_points_num = + current_polygon_points_num - current_polygon_border_indices.size() + 1; + for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { + const int target_poly_idx = current_polygon_border_indices.front() + + poly_idx * (is_polygon_opposite_direction ? -1 : 1); + expanded_bound.push_back( + (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)]); + } + } + current_polygon = std::nullopt; + current_polygon_border_indices.clear(); + } + } + + return expanded_bound; +} + +std::vector getBoundWithIntersectionAreas( + const std::vector & original_bound, + const std::shared_ptr & route_handler, + const std::vector & drivable_lanes, const bool is_left) +{ const auto extract_bound_from_polygon = [](const auto & polygon, const auto start_idx, const auto end_idx, const auto clockwise) { - std::vector ret; + std::vector ret{}; for (size_t i = start_idx; i != end_idx; i = clockwise ? i + 1 : i - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[i])); + ret.push_back(polygon[i]); if (i + 1 == polygon.size() && clockwise) { if (end_idx == 0) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = 0; @@ -1609,7 +1659,7 @@ std::vector calcBound( if (i == 0 && !clockwise) { if (end_idx == polygon.size() - 1) { - ret.push_back(lanelet::utils::conversion::toGeomMsgPt(polygon[end_idx])); + ret.push_back(polygon[end_idx]); return ret; } i = polygon.size() - 1; @@ -1617,34 +1667,30 @@ std::vector calcBound( } } + ret.push_back(polygon[end_idx]); + return ret; }; - // If no need to expand with polygons, return here. - std::vector output_points; - const auto bound_points = convert_to_points(drivable_lanes); - if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { - for (const auto & point : bound_points) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); - } - return motion_utils::removeOverlapPoints(output_points); - } + std::vector expanded_bound = original_bound; - std::optional current_polygon{std::nullopt}; - std::vector current_polygon_border_indices; + // expand drivable area by using intersection area. for (const auto & drivable_lane : drivable_lanes) { - // extract target lane and bound. - const auto bound_lane = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; - const auto bound = is_left ? bound_lane.leftBound3d() : bound_lane.rightBound3d(); + const auto edge_lanelet = is_left ? drivable_lane.left_lane : drivable_lane.right_lane; + const auto lanelet_bound = is_left ? edge_lanelet.leftBound3d() : edge_lanelet.rightBound3d(); - if (bound.size() < 2) { + if (lanelet_bound.size() < 2) { continue; } - // expand drivable area by intersection areas. - const std::string id = bound_lane.attributeOr("intersection_area", "else"); - const auto use_intersection_area = enable_expanding_intersection_areas && id != "else"; - if (use_intersection_area) { + const std::string id = edge_lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + continue; + } + + // Step1. extract intersection partial bound. + std::vector intersection_bound{}; + { const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); @@ -1652,96 +1698,137 @@ std::vector calcBound( boost::geometry::is_valid(lanelet::utils::to2D(polygon.basicPolygon())); const auto is_clockwise_iteration = is_clockwise_polygon ? is_left : !is_left; - const auto start_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.front().id(); - }); + const auto intersection_bound_itr_init = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.front().id(); }); - const auto end_itr = std::find_if(polygon.begin(), polygon.end(), [&bound](const auto & p) { - return p.id() == bound.back().id(); - }); + const auto intersection_bound_itr_last = std::find_if( + polygon.begin(), polygon.end(), + [&lanelet_bound](const auto & p) { return p.id() == lanelet_bound.back().id(); }); - if (start_itr == polygon.end() || end_itr == polygon.end()) { + if ( + intersection_bound_itr_init == polygon.end() || + intersection_bound_itr_last == polygon.end()) { continue; } // extract line strings between start_idx and end_idx. - const size_t start_idx = std::distance(polygon.begin(), start_itr); - const size_t end_idx = std::distance(polygon.begin(), end_itr); - for (const auto & point : - extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration)) { - output_points.push_back(point); - } + const size_t start_idx = std::distance(polygon.begin(), intersection_bound_itr_init); + const size_t end_idx = std::distance(polygon.begin(), intersection_bound_itr_last); - continue; + intersection_bound = + extract_bound_from_polygon(polygon, start_idx, end_idx, is_clockwise_iteration); } - if (!enable_expanding_hatched_road_markings) { - for (const auto & point : bound) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); - } + // Step2. check shared bound point. + const auto shared_point_itr_init = + std::find_if(expanded_bound.begin(), expanded_bound.end(), [&](const auto & p) { + return std::any_of( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + + const auto shared_point_itr_last = + std::find_if(expanded_bound.rbegin(), expanded_bound.rend(), [&](const auto & p) { + return std::any_of( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & point) { return point.id() == p.id(); }); + }); + + if ( + shared_point_itr_init == expanded_bound.end() || + shared_point_itr_last == expanded_bound.rend()) { continue; } - // expand drivable area by hatched road markings. - for (size_t bound_point_idx = 0; bound_point_idx < bound.size(); ++bound_point_idx) { - const auto & bound_point = bound[bound_point_idx]; - const auto polygon = getPolygonByPoint(route_handler, bound_point, "hatched_road_markings"); - - bool will_close_polygon{false}; - if (!current_polygon) { - if (!polygon) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(bound_point)); - } else { - // There is a new additional polygon to expand - current_polygon = polygon; - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } - } else { - if (!polygon) { - will_close_polygon = true; - } else { - current_polygon_border_indices.push_back( - get_corresponding_polygon_index(*current_polygon, bound_point.id())); - } - } + // Step3. overwrite duplicate drivable bound by intersection bound. + { + const auto trim_point_itr_init = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_init->id(); }); - if (bound_point_idx == bound_points.size() - 1 && current_polygon) { - // If drivable lanes ends earlier than polygon, close the polygon - will_close_polygon = true; + const auto trim_point_itr_last = std::find_if( + intersection_bound.begin(), intersection_bound.end(), + [&](const auto & p) { return p.id() == shared_point_itr_last->id(); }); + + if ( + trim_point_itr_init == intersection_bound.end() || + trim_point_itr_last == intersection_bound.end()) { + continue; } - if (will_close_polygon) { - // The current additional polygon ends to expand - if (current_polygon_border_indices.size() == 1) { - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[current_polygon_border_indices.front()])); - } else { - const size_t current_polygon_points_num = current_polygon->size(); - const bool is_polygon_opposite_direction = [&]() { - const size_t modulo_diff = mod( - static_cast(current_polygon_border_indices[1]) - - static_cast(current_polygon_border_indices[0]), - current_polygon_points_num); - return modulo_diff == 1; - }(); + std::vector tmp_bound{}; - const int target_points_num = - current_polygon_points_num - current_polygon_border_indices.size() + 1; - for (int poly_idx = 0; poly_idx <= target_points_num; ++poly_idx) { - const int target_poly_idx = current_polygon_border_indices.front() + - poly_idx * (is_polygon_opposite_direction ? -1 : 1); - output_points.push_back(lanelet::utils::conversion::toGeomMsgPt( - (*current_polygon)[mod(target_poly_idx, current_polygon_points_num)])); - } + tmp_bound.insert(tmp_bound.end(), expanded_bound.begin(), shared_point_itr_init); + tmp_bound.insert(tmp_bound.end(), trim_point_itr_init, trim_point_itr_last); + tmp_bound.insert( + tmp_bound.end(), std::next(shared_point_itr_last).base(), expanded_bound.end()); + + expanded_bound = tmp_bound; + } + } + + return expanded_bound; +} + +// calculate bounds from drivable lanes and hatched road markings +std::vector calcBound( + const std::shared_ptr route_handler, + const std::vector & drivable_lanes, + const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, + const bool is_left) +{ + // a function to convert drivable lanes to points without duplicated points + const auto convert_to_points = [&](const std::vector & drivable_lanes) { + constexpr double overlap_threshold = 0.01; + + std::vector points; + for (const auto & drivable_lane : drivable_lanes) { + const auto bound = + is_left ? drivable_lane.left_lane.leftBound3d() : drivable_lane.right_lane.rightBound3d(); + for (const auto & point : bound) { + if ( + points.empty() || + overlap_threshold < (points.back().basicPoint2d() - point.basicPoint2d()).norm()) { + points.push_back(point); } - current_polygon = std::nullopt; - current_polygon_border_indices.clear(); } } + return points; + }; + + const auto to_ros_point = [](const std::vector & bound) { + std::vector ret{}; + std::for_each(bound.begin(), bound.end(), [&](const auto & p) { + ret.push_back(lanelet::utils::conversion::toGeomMsgPt(p)); + }); + return ret; + }; + + // Step1. create drivable bound from drivable lanes. + std::vector bound_points = convert_to_points(drivable_lanes); + + // Step2. if there is no drivable area defined by polygon, return original drivable bound. + if (!enable_expanding_hatched_road_markings && !enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); + } + + // Step3. if there are hatched road markings, expand drivable bound with the polygon. + if (enable_expanding_hatched_road_markings) { + bound_points = getBoundWithHatchedRoadMarkings(bound_points, route_handler); + } + + if (!enable_expanding_intersection_areas) { + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } - return motion_utils::removeOverlapPoints(output_points); + // Step4. if there are intersection areas, expand drivable bound with the polygon. + { + bound_points = + getBoundWithIntersectionAreas(bound_points, route_handler, drivable_lanes, is_left); + } + + return motion_utils::removeOverlapPoints(to_ros_point(bound_points)); } void makeBoundLongitudinallyMonotonic( @@ -1915,19 +2002,27 @@ void makeBoundLongitudinallyMonotonic( return bound; } - std::vector ret; + std::vector ret = bound; + auto itr = std::next(ret.begin()); + while (std::next(itr) != ret.end()) { + const auto p1 = *std::prev(itr); + const auto p2 = *itr; + const auto p3 = *std::next(itr); - for (size_t i = 0; i < bound.size(); i++) { - const auto & p_new = bound.at(i); - const auto duplicated_points_itr = std::find_if( - ret.begin(), ret.end(), - [&p_new](const auto & p) { return calcDistance2d(p, p_new) < 0.1; }); + const std::vector vec_1to2 = {p2.x - p1.x, p2.y - p1.y, p2.z - p1.z}; + const std::vector vec_3to2 = {p2.x - p3.x, p2.y - p3.y, p2.z - p3.z}; + const auto product = + std::inner_product(vec_1to2.begin(), vec_1to2.end(), vec_3to2.begin(), 0.0); - if (duplicated_points_itr != ret.end() && std::next(duplicated_points_itr, 2) == ret.end()) { - ret.erase(duplicated_points_itr, ret.end()); - } + const auto dist_1to2 = tier4_autoware_utils::calcDistance3d(p1, p2); + const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); - ret.push_back(p_new); + constexpr double epsilon = 1e-3; + if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { + itr = ret.erase(itr); + } else { + itr++; + } } return ret; From 1c2c59d0ab81400b692aa8d9c3b595be0222b9ab Mon Sep 17 00:00:00 2001 From: shulanbushangshu <102840938+shulanbushangshu@users.noreply.github.com> Date: Tue, 3 Oct 2023 13:16:21 +0800 Subject: [PATCH 020/206] feat: add common interface utils test (#5173) * add interface utils test Signed-off-by: jack.song * style(pre-commit): autofix --------- Signed-off-by: jack.song Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../component_interface_utils/CMakeLists.txt | 20 +++++ common/component_interface_utils/package.xml | 1 + .../test/test_component_interface_utils.cpp | 76 +++++++++++++++++++ 3 files changed, 97 insertions(+) mode change 100644 => 100755 common/component_interface_utils/package.xml create mode 100644 common/component_interface_utils/test/test_component_interface_utils.cpp diff --git a/common/component_interface_utils/CMakeLists.txt b/common/component_interface_utils/CMakeLists.txt index d4f6343e381c7..435d5535e0a37 100644 --- a/common/component_interface_utils/CMakeLists.txt +++ b/common/component_interface_utils/CMakeLists.txt @@ -3,4 +3,24 @@ project(component_interface_utils) find_package(autoware_cmake REQUIRED) autoware_package() + +include_directories( + include + SYSTEM + ${rclcpp_INCLUDE_DIRS} + ${rosidl_runtime_cpp_INCLUDE_DIRS} + ${rcl_INCLUDE_DIRS} + ${autoware_adapi_v1_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_component_interface_utils + test/test_component_interface_utils.cpp + ) + + target_include_directories(test_component_interface_utils + PRIVATE include + ) +endif() + ament_auto_package() diff --git a/common/component_interface_utils/package.xml b/common/component_interface_utils/package.xml old mode 100644 new mode 100755 index a1803a35cc1c9..dc82fda3f5c64 --- a/common/component_interface_utils/package.xml +++ b/common/component_interface_utils/package.xml @@ -19,6 +19,7 @@ rclcpp rclcpp_components + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/common/component_interface_utils/test/test_component_interface_utils.cpp b/common/component_interface_utils/test/test_component_interface_utils.cpp new file mode 100644 index 0000000000000..3c41f4a85b4f9 --- /dev/null +++ b/common/component_interface_utils/test/test_component_interface_utils.cpp @@ -0,0 +1,76 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "component_interface_utils/rclcpp/exceptions.hpp" +#include "component_interface_utils/specs.hpp" +#include "component_interface_utils/status.hpp" +#include "gtest/gtest.h" + +TEST(interface, utils) +{ + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_back; + code_back = service.status(); + EXPECT_EQ(code_back.code, code); + EXPECT_EQ(code_back.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + + ResponseStatusCode code = 10; + const std::string message = "test_exception"; + ServiceException service(code, message); + ResponseStatus code_set; + service.set(code_set); + EXPECT_EQ(code_set.code, code); + EXPECT_EQ(code_set.message, message); + } + + { + using component_interface_utils::ServiceException; + using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; + using ResponseStatusCode = ResponseStatus::_code_type; + using component_interface_utils::status::copy; + + class status_test + { + public: + status_test(ResponseStatusCode code, const std::string & message, bool success = false) + { + status.code = code; + status.message = message; + status.success = success; + } + ResponseStatus status; + }; + + const status_test status_in(10, "test_exception", true); + auto status_copy = std::make_shared(100, "test_exception_copy", false); + copy(&status_in, status_copy); + + EXPECT_EQ(status_in.status.code, status_copy->status.code); + EXPECT_EQ(status_in.status.message, status_copy->status.message); + EXPECT_EQ(status_in.status.success, status_copy->status.success); + } +} From 9aa2e5a8fdabe62c86dfec5d56de128f5e6ab6aa Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Tue, 3 Oct 2023 17:56:46 +0900 Subject: [PATCH 021/206] feat(goal_planner): secure lateral distance for path planning with corresponding to the centrifugal force (#5196) * add feature to plan the path with considering latral overshoot Signed-off-by: Yuki Takagi * separate a function in "safety_check" in order to that "goal_planner_module" can handle the ego_polygons Signed-off-by: Yuki Takagi * add visualization codes fix the handling miss of sign of value Signed-off-by: Yuki Takagi * refactor codes based on comments in the PR Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi --- .../goal_planner/goal_planner_module.hpp | 1 + .../path_safety_checker/safety_check.hpp | 14 +++-- .../goal_planner/goal_planner_module.cpp | 63 ++++++++++++------- .../path_safety_checker/safety_check.cpp | 40 +++++++++--- 4 files changed, 82 insertions(+), 36 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index eb6ea5996cc6f..a82cff16756f8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -106,6 +106,7 @@ struct FreespacePlannerDebugData struct GoalPlannerDebugData { FreespacePlannerDebugData freespace_planner{}; + std::vector ego_polygons_expanded{}; }; class GoalPlannerModule : public SceneModuleInterface diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 9cc4afe22bdce..4983060aa1c0a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -110,6 +110,10 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin); + /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -133,14 +137,12 @@ std::vector getCollidedPolygons( const double hysteresis_factor, CollisionCheckDebug & debug); /** - * @brief Check collision between ego path footprints with extra longitudinal stopping margin and - * objects. + * @brief Check collision between ego polygons with margin and objects. * @return Has collision or not */ -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double margin, const double max_stopping_margin); +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin); } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 574a1a38fc66a..ce88760212d90 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1296,28 +1296,32 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } } - if (parameters_->use_object_recognition) { - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - const auto [pull_over_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( - pull_over_lane_objects, parameters_->th_moving_object_velocity); - const auto common_parameters = planner_data_->parameters; - const double base_link2front = common_parameters.base_link2front; - const double base_link2rear = common_parameters.base_link2rear; - const double vehicle_width = common_parameters.vehicle_width; - if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, - parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, - parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { - return true; - } + if (!parameters_->use_object_recognition) { + return false; } - return false; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); + const auto common_parameters = planner_data_->parameters; + const double base_link2front = common_parameters.base_link2front; + const double base_link2rear = common_parameters.base_link2rear; + const double vehicle_width = common_parameters.vehicle_width; + + const auto ego_polygons_expanded = + utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin( + path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, + parameters_->object_recognition_collision_check_max_extra_stopping_margin); + debug_data_.ego_polygons_expanded = ego_polygons_expanded; + + return utils::path_safety_checker::checkCollisionWithMargin( + ego_polygons_expanded, pull_over_lane_stop_objects, + parameters_->object_recognition_collision_check_margin); } bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const @@ -1686,6 +1690,24 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + + auto marker = tier4_autoware_utils::createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "detection_polygons", 0, Marker::LINE_LIST, + tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), + tier4_autoware_utils::createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + for (const auto & ego_polygon : debug_data_.ego_polygons_expanded) { + for (size_t ep_idx = 0; ep_idx < ego_polygon.outer().size(); ++ep_idx) { + const auto & current_point = ego_polygon.outer().at(ep_idx); + const auto & next_point = ego_polygon.outer().at((ep_idx + 1) % ego_polygon.outer().size()); + + marker.points.push_back( + tier4_autoware_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + tier4_autoware_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker_.markers.push_back(marker); } // safety check if (parameters_->safety_check_params.enable_safety_check) { @@ -1695,7 +1717,6 @@ void GoalPlannerModule::setDebugData() add(createPredictedPathMarkerArray( ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9)); } - if (goal_planner_data_.filtered_objects.objects.size() > 0) { add(createObjectsMarkerArray( goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 4e671f9f4b599..b44a3f841035d 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -364,27 +364,49 @@ std::vector getCollidedPolygons( return collided_polygons; } -bool checkCollisionWithExtraStoppingMargin( - const PathWithLaneId & ego_path, const PredictedObjects & dynamic_objects, - const double base_to_front, const double base_to_rear, const double width, - const double maximum_deceleration, const double collision_check_margin, - const double max_extra_stopping_margin) + +std::vector generatePolygonsWithStoppingAndInertialMargin( + const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, + const double width, const double maximum_deceleration, const double max_extra_stopping_margin) { - for (const auto & p : ego_path.points) { + std::vector polygons; + const auto curvatures = motion_utils::calcCurvature(ego_path.points); + + for (size_t i = 0; i < ego_path.points.size(); ++i) { + const auto p = ego_path.points.at(i); + const double extra_stopping_margin = std::min( std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, max_extra_stopping_margin); + double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps * + std::abs(p.point.longitudinal_velocity_mps); + extra_lateral_margin = + std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + + const auto lateral_offset_pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); const auto ego_polygon = tier4_autoware_utils::toFootprint( - p.point.pose, base_to_front + extra_stopping_margin, base_to_rear, width); + lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear, + width + std::abs(extra_lateral_margin)); + polygons.push_back(ego_polygon); + } + return polygons; +} +bool checkCollisionWithMargin( + const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, + const double collision_check_margin) +{ + for (const auto & ego_polygon : ego_polygons) { for (const auto & object : dynamic_objects.objects) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); const double distance = boost::geometry::distance(obj_polygon, ego_polygon); - if (distance < collision_check_margin) return true; + if (distance < collision_check_margin) { + return true; + } } } - return false; } } // namespace behavior_path_planner::utils::path_safety_checker From 68544c73bffca2b5acbaab299303f64653b6be8d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Tue, 3 Oct 2023 12:13:21 +0300 Subject: [PATCH 022/206] ci(build-and-test-differential): remove sync for the build-and-test-differential workflows (#5200) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .github/sync-files.yaml | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 4a1f20cada359..f9cb3065f12a7 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -42,20 +42,6 @@ - \"\" - -cuda include:" {source} - - source: .github/workflows/build-and-test-differential.yaml - pre-commands: | - sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - - sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} - sd -- \ - " include:" \ - " container-suffix: - - \"\" - - -cuda - include:" {source} - - sd "^ container: ghcr.io/autowarefoundation/autoware-universe:(\w+)-latest\$" \ - " container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest-cuda" {source} - source: .github/workflows/build-and-test-differential-self-hosted.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} From ac06a5c2e05bc166478d1ff5e0d730471089a41c Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Tue, 3 Oct 2023 09:24:59 +0000 Subject: [PATCH 023/206] chore: sync files (#4372) Signed-off-by: GitHub Co-authored-by: xmfcx --- mkdocs.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mkdocs.yaml b/mkdocs.yaml index 2ca8b3c86ad43..a72529fbe959a 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -55,8 +55,7 @@ plugins: regex: - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$ - ^(.*/)?[^.]*$ - - macros: - module_name: mkdocs_macros + - macros - mkdocs-video - same-dir - search From 8a0ff1cfaa2c5a8afd47a280bd67db435f799146 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 3 Oct 2023 19:01:42 +0900 Subject: [PATCH 024/206] revert: "chore: sync files (#4372)" (#5213) Signed-off-by: Ryohsuke Mitsudome --- mkdocs.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mkdocs.yaml b/mkdocs.yaml index a72529fbe959a..2ca8b3c86ad43 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -55,7 +55,8 @@ plugins: regex: - ^(?!(.*/)?assets/).*\.(?!(.*\.)?md|(.*\.)?svg|(.*\.)?png|(.*\.)?gif|(.*\.)?jpg).*$ - ^(.*/)?[^.]*$ - - macros + - macros: + module_name: mkdocs_macros - mkdocs-video - same-dir - search From 6155f1ff63b95492f5d62250bda03334efe416c7 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Oct 2023 09:49:56 +0900 Subject: [PATCH 025/206] feat(intersection): aggressively peek into attention area if traffic light does not exist (#5192) Signed-off-by: Mamoru Sobue --- .../README.md | 93 +- .../config/intersection.param.yaml | 3 + .../docs/data-structure.drawio.svg | 771 ++++++++++++++++ .../intersection-attention-ll-rr.drawio.svg | 134 ++- .../intersection-attention-lr-rl.drawio.svg | 832 ++++++++++++++---- ...intersection-attention-straight.drawio.svg | 152 +++- .../docs/intersection-attention.drawio.svg | 348 ++++++-- .../docs/occlusion-without-tl.drawio.svg | 518 +++++++++++ .../docs/stuck-vehicle.drawio.svg | 42 +- .../package.xml | 4 - .../src/debug.cpp | 12 +- .../src/manager.cpp | 20 +- .../src/scene_intersection.cpp | 337 ++++--- .../src/scene_intersection.hpp | 45 +- .../src/util.cpp | 2 +- .../src/util.hpp | 8 - .../src/util_type.hpp | 1 + 17 files changed, 2803 insertions(+), 519 deletions(-) create mode 100644 planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg create mode 100644 planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index e8a823a7e790e..16459bb64074b 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -24,7 +24,7 @@ This module is activated when the path contains the lanes with `turn_direction` ### Attention area -The `Attention Area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority. +The `attention area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `common.attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority(`yield lane`). `Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection. @@ -54,10 +54,10 @@ For [stuck vehicle detection](#stuck-vehicle-detection) and [collision detection Objects that satisfy all of the following conditions are considered as target objects (possible collision objects): -- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `attention_area_margin`) . +- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `common.attention_area_margin`) . - (Optional condition) The center of gravity is in the **intersection area**. - To deal with objects that is in the area not covered by the lanelets in the intersection. -- The posture of object is **the same direction as the attention lane** (threshold = `attention_area_angle_threshold`). +- The posture of object is **the same direction as the attention lane** (threshold = `common.attention_area_angle_threshold`). - Not being **in the adjacent lanes of the ego vehicle**. #### Stuck Vehicle Detection @@ -68,31 +68,31 @@ If there is any object on the path in inside the intersection and at the exit of #### Collision detection -The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough _margin_, it will insert a stopline on the _path_. +The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough margin, this module inserts a stopline on the path. 1. calculate the time interval that the ego vehicle is in the intersection. This time is set as $t_s$ ~ $t_e$ -2. extract the predicted path of the target object whose confidence is greater than `min_predicted_path_confidence`. +2. extract the predicted path of the target object whose confidence is greater than `collision_detection.min_predicted_path_confidence`. 3. detect collision between the extracted predicted path and ego's predicted path in the following process. 1. obtain the passing area of the ego vehicle $A_{ego}$ in $t_s$ ~ $t_e$. - 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_start_margin_time` ~ $t_e$ + `collision_end_margin_time` for each predicted path (\*1). + 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_detection.collision_start_margin_time` ~ $t_e$ + `collision_detection.collision_end_margin_time` for each predicted path (\*1). 3. check if $A_{ego}$ and $A_{target}$ polygons are overlapped (has collision). 4. when a collision is detected, the module inserts a stopline. 5. If ego is over the `pass_judge_line`, collision checking is not processed to avoid sudden braking and/or unnecessary stop in the middle of the intersection. -The parameters `collision_start_margin_time` and `collision_end_margin_time` can be interpreted as follows: +The parameters `collision_detection.collision_start_margin_time` and `collision_detection.collision_end_margin_time` can be interpreted as follows: -- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_start_margin_time`. -- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_end_margin_time`. +- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_start_margin_time`. +- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_end_margin_time`. -If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless _safe_ judgement continues for a certain period to prevent the chattering of decisions. +If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions. #### Stop Line Automatic Generation -If a stopline is associated with the intersection lane, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention lane is defined as the position of the stop line for the vehicle front. +If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front. #### Pass Judge Line -To avoid a rapid braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) are needed to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. +To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, this module does not insert stopline after passing the default stopline position. ### Occlusion detection @@ -104,6 +104,66 @@ The occlusion is detected as the common area of occlusion attention area(which i If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. +In there are no traffic lights associated with the lane, the ego vehicle will make a brief stop at the _default stop line_ and the position where the vehicle heading touches the attention area for the first time(which is denoted as _first attention stop line_). After stopping at the _first attention area stop line_ this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and the position which is `occlusion.absence_traffic_light.maximum_peeking_distance` ahead of _first attention area stop line_ while occlusion is not cleared. If collision is detected, ego will instantly stop. Once the occlusion is cleared or ego passed `occlusion.absence_traffic_light.maximum_peeking_distance` this module does not detect collision and occlusion because ego vehicle is already inside the intersection. + +![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) + +### Data Structure + +#### `IntersectionLanelets` + +```plantuml +@startuml +entity IntersectionLanelets { + * conflicting lanes/area + -- + * first conflicting area + The conflicting lane area which the path intersects first + -- + * attention lanes/area + -- + * first attention lane area + The attention lane area which the path intersects first + -- + * occlusion attention lanes/area + Part of attention lanes/area for occlusion detection + -- + * is_priortized: bool + If ego vehicle has priority in current traffic light context +} +@enduml +``` + +#### `IntersectionStopLines` + +Each stop lines are generated from interpolated path points to obtain precise positions. + +```plantuml +@startuml +entity IntersectionStopLines { + * closest_idx: size_t + closest path point index for ego vehicle + -- + * stuck_stop_line: size_t + stop line index on stuck vehicle detection + -- + * default_stop_line: size_t + If defined on the map, its index on the path. Otherwise generated before first_attention_stop_line + -- + * first_attention_stop_line + The index of the first path point which is inside the attention area + -- + * occlusion_peeking_stop_line + The index of the path point for the peeking limit position + -- + * pass_judge_line + The index of the path point which is before first_attention_stop_line/occlusion_peeking_stop_line by braking distance +} +@enduml +``` + +![data structure](./docs/data-structure.drawio.svg) + ### Module Parameters | Parameter | Type | Description | @@ -115,7 +175,6 @@ If the nearest occlusion cell value is below the threshold, occlusion is detecte | `common.intersection_velocity` | double | [m/s] velocity profile for pass judge calculation | | `common.intersection_max_accel` | double | [m/s^2] acceleration profile for pass judge calculation | | `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | -| `common.path_interpolation_ds` | double | [m] path interpolation interval | | `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | | `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | | `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | @@ -128,7 +187,13 @@ If the nearest occlusion cell value is below the threshold, occlusion is detecte | `occlusion.peeking_offset` | double | [m] the offset of the front of the vehicle into the attention area for peeking to occlusion | | `occlusion.min_vehicle_brake_for_rss` | double | [m/s] assumed minimum brake of the vehicle running from behind the occlusion | | `occlusion.max_vehicle_velocity_for_rss` | double | [m/s] assumed maximum velocity of the vehicle running from behind the occlusion | -| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | + +#### For developers only + +| Parameter | Type | Description | +| ------------------------------ | ------ | ---------------------------------------------------------------------- | +| `common.path_interpolation_ds` | double | [m] path interpolation interval | +| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | ### How to turn parameters diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 4042adeb3623e..5e74e837bf186 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -54,6 +54,9 @@ ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h stop_release_margin_time: 1.5 # [s] temporal_stop_before_attention_area: false + absence_traffic_light: + creep_velocity: 1.388 # [m/s] + maximum_peeking_distance: 6.0 # [m] enable_rtc: intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg new file mode 100644 index 0000000000000..fb512902ef856 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg @@ -0,0 +1,771 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + next + + +
+
+
+ + next + + + + + + + + +
+
+
+ + + prev + + +
+
+
+ + prev + + + + + + + + +
+
+
+ + + ego_or_entry2exit + + +
+
+
+ + ego_or_entry2exit + + + + + + + + +
+
+
+ + + entry2ego + + +
+
+
+ + entry2ego + + + + + + + +
+
+
+ + IntersectionStopLines + +
+
+
+ + IntersectionStopLines + + + + + +
+
+
+ + PathLanelets + +
+
+
+ + PathLanelets + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg index d4d709eee6b99..51e926fe266d6 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg @@ -8,12 +8,48 @@ width="3723px" height="2401px" viewBox="-0.5 -0.5 3723 2401" - content="<mxfile host="Electron" modified="2023-06-06T10:28:31.375Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="M_P_fA_OZlKNZHzEOQxb" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + content="<mxfile host="Electron" modified="2023-10-03T02:44:21.616Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="wkqIuFIE5UyEHdVd8sjR" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + + + + + + + + + - + @@ -51,7 +87,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + yield lane - - + + @@ -749,10 +785,10 @@ - - + + - + - attention area + attention lane - attention area + attention lane @@ -1141,12 +1177,12 @@
- attention area + attention lane
- attention area + attention lane @@ -1226,9 +1262,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg index 2d590b9fd51ca..25a7c6b519f96 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg @@ -5,10 +5,10 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3729px" + width="3727px" height="2651px" - viewBox="-0.5 -0.5 3729 2651" - content="<mxfile host="Electron" modified="2023-06-06T10:27:24.636Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="EZYouzMJcjefpLDW8VVp" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + viewBox="-0.5 -0.5 3727 2651" + content="<mxfile host="Electron" modified="2023-10-03T03:32:56.314Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="aYRnTHIqVcSkLQHKGcXP" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -25,13 +25,13 @@ transform="rotate(-130,954.7,670.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -50,8 +50,7 @@ - - + @@ -88,9 +87,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - - - - + + + + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + @@ -436,8 +435,8 @@ ego lane - - + + @@ -474,16 +473,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -548,12 +547,12 @@ transform="rotate(225,2890.14,695.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - + + - - - + + + - - + + - + @@ -753,13 +752,13 @@ transform="rotate(-130,954.7,1832.3)" pointer-events="none" /> - - - + + + - + - - - + + + @@ -779,7 +778,7 @@ - + - + - + - + - + - + - + - - - + + + - - - - - - - - + - - + + - - + + - + - + - - - + + + - + - + - + - - + + - + - + - + - + - + @@ -1150,8 +1118,8 @@ ego lane - - + + @@ -1186,16 +1154,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -1224,12 +1192,12 @@ transform="rotate(225,2890.14,1857.64)" pointer-events="none" /> - + - - + + - attention area + attention lane - attention area + attention lane - - - - - + + + + + - - + + - + @@ -1376,6 +1344,494 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg index d33674a257d9b..640eba618fa49 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg @@ -5,41 +5,42 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="3389px" - height="1411px" - viewBox="-0.5 -0.5 3389 1411" - content="<mxfile host="Electron" modified="2023-06-06T10:31:30.247Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="U1JUqxOsfTr30viw8iHe" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="3390px" + height="1412px" + viewBox="-0.5 -0.5 3390 1412" + content="<mxfile host="Electron" modified="2023-10-03T04:48:10.725Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="02kGXKyoYgRU_RdR3ghD" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > + - + + - - + - + - + @@ -390,7 +391,7 @@ - + - + - + - + - + - + - - - + + + - + attention area - - - - + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg index 3e9826fd84ad6..57552f586e63b 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg @@ -6,9 +6,9 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="3707px" - height="2193px" - viewBox="-0.5 -0.5 3707 2193" - content="<mxfile host="Electron" modified="2023-06-06T08:17:01.045Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="0BpzzxhIQKLXKpqq79kr" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + height="2195px" + viewBox="-0.5 -0.5 3707 2195" + content="<mxfile host="Electron" modified="2023-10-03T05:15:42.124Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="WBrLRHcQvF5FOJ4jcZAE" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">7V1bd+M2kv41Pmf2QTy4Xx7bfcl9JiedTif7kiNbsluJ2vLI6nT3/voFJYEiAQgEKYCEbCvZnViiQBH11YeqQqHqAr/8+OWb9fT+w0+r2Xx5gcDsywV+dYEQwoDiAqn/Kt/8unsTIsBgIXZv3q4Xs/3bhzfeLv5vvn8T7N/9tJjNHxoXblar5WZx33zzenV3N7/eNN6brterz83LblbL5l3vp7dz642319Olflc/RPn++8Vs82H3vkD88P6388XtB31vyOTuk49TffH+WR4+TGerz7W38OsL/HK9Wm12//Xxy8v5spxFPTO/vfyGfP3vu9mvL2b3b968fPXH/377ZrIb7E2Xr1SPtp7fbXoPffntX3fk6k9y+Zlfvpfz3ybo328mYv9s/0yXn/ZTtn/YzVc9h+vVp7vZvBwFXuDLzx8Wm/nb++l1+elnBR713ofNx+X+4+X0ar68nF7/fbv92svVcrXeDoPfbF/qkofNevX3vPYJ2L7UJzeru80eQ5ju/65d94rLy911i+VSv3+3ulPXX06Xi9s79ee1mqK5ev8ycM72c/vPfL2Zf6khaT+H38xXH+eb9Vd1yf7TCSQIk4LuvrdXjQmHYD/S5wPIsJ7dDzWAIf3mdA/t2+oWB+Gp/9jLr4MsteieZRkuSwYpMCWpuWsQSX78ZTX95urtHz/8yT7PFr8SxjcPEynbJVnKpJTXerWZbharcr4mEpw+WRUXT6/0rUDLJFLAaSEPL9GcUcoxVB8zCrEiVwSxNb0UkAJhjCjDmHIuqT3ZBMhC0MMYACeaeyHa576ctIVaZH4sFeTn1cNiL4Cr1Waz+qhkoC94sUfyZmWollpL7svBPn65Ldff4mr6sLgulNCUjtxdv14u1QI53+qXeuvF3e32zqAQDHEBMScAYqn+T10xv5sdPlfYxYwreUjOGSWyVKb1tV73QMG4oXF1TawjiQBLuffKuVJ0sdiUsqVutPkRHaybgjRR5NBKKKANFMhlKmQEaOWBX0E7vwYIYnQxQN4uBySwLQeWjBw7mSzdxXC5/cfFrCGCUOo8W8wPS93+qmTymRjyUUuVvXpxMaR8YLt8StIqrfvDBNXk0RSeZWFUarL7RJMbsWUx/7LY/L4ftPzvP7YcSPd/vfqyv8P2j6/6jzs1B7/X/6h9q/zz8LXtX4fvzd4syol61UMdH1af1tfzAOpRq8Ht3DeivnA+a/hFNnwaq6+NDv3eer5UOvBP08dyQWZ/h59XC/XEFToJRQ10YtIcYffg+y/VfRdzHNAcRxluzYF2E2MNtAVw9dQnYDrAtK5h+no5fVDreWJYgwIx1IA2EjwNuNUgP8/XCzWNpT2eBuIiEOKa/zOBuHIHG9CEoifGpTGOqSupIY7bIX79af1P5TwOy+GgE4cPwcW54E/buxo3tB/8IOSNcUwz+gj8FASmX2uX3ZcXPPjUxflzj/4q1LwctVxuPASWhqrsfm5UxREBiuNw1s3Yh60Vrk8M7bhRfuK3e5WzcZ7c7yfAcA4oY/uFuQ5+Rmz0M1lID9ZPc9RomtW6TlPBIgqkH42iTFgFF4gQIjigjDJO9eKg1aonx1CG63GipmrjYU06DdQQtS3V7DetszWVq3uIUIwUfCPAH3wjtkZCQFwee0KVZLx9tmPHTiaY6UB07crdy6XBlePfT4MrQAX77gTYsRQ2uGQC4p2DkWVfB6WjiFp9DA3XTNgYAmzYNT0J2GAHc5jElAtB8L7G4DRKJG1sUhjxBsokK1mWYgmAIlk7yEalQ/6YkUKI6ltcGGtcNC2GIMAKfVTbFs4wbextiwNig0kdswIf9qlMIGFe2+gSgOz9mRF3NaC24YeMp48vJoiUnJgtG0scRNmsTgcmVfwcggD35Vw3OHpIalISM8dEMIEBVxwKmmJDkBaUHjhW2jJEWBR67R9IhgHexSkBtEqGiQJo2gxTax43TDFSvRG0FbL9rGfwuEJLa1CuUppW0666MhPbjgPfekHKDLb6gtHP7vPfBCr1aK5KA5uFAc5htNBzB80ZciswDdTJGUEdEuBFYXAgG3lvI0wLLAjrXcPc/kcVzidNGqWGIJGfP/yiBJPEBrooIDlTBSRYNmHZNKRMVPYMMjC/mptrTbIYhDNJNnizZvAIBGPKdaxNTWPWJlw5xwU5xCf2wKpn+cjG1yF2BBKxYNGzKP/93Wr2hfzzYUEu362/+5v/ya++ncCnFo5wBqDVBLt4csblVdhWkRfCXUISTWA0d+0gB8WQYQg3YBJHIZzycTi9I4hnwlDBGeMEM6bsfVHtoWr5AJd8EHPIJ4ZD6xZP4piEW31EFvIpQ0UuYtXLppAu8XDHMp5OPInDDWlzLrsJrm6feakkE6OLe9EDedHXzPKjkloDx7Os3NM+pBffAZAt3rgPapkgqLOkw11l78jYBmck99j/RIf7xnKJ3YBN7xF3gWmsnW8foOseq3edzwT53QEaCn01pFepcEL6nP786vL3P8D9j8vJh3e/bNYvrz7KEMf0FANLMo5fsDADq7x2ykIMrGjHMKWxFzdhXNp5S0SvMQmSY96hf14yPlvBW0L/pr9/+Iku3qcWyvGEz8rGrYlLBtm88UQCScFMqQhKC2xvsDGdvF4XDIeFT+NDBXPF7pffff/7jzOymixevf/++//wVxOMnrJklMdhHsvbSsaRTaAkU0DH1mck4TifqVv6rW37NRdL3HkyM1m8lEWMMTDF1HcLr5Q7MkbjkBkBvEj2metm6qfjqEaZU4SnOrKPGTtEoHjYYZAZvyoldsg+mpgUOwEO6NPADoPQ2D5QytvTUFajUWRuRkBmsFg87FDErZ9O0mOnmy8Ygp3Z9OFDFeuw90rflP+cMcSYJqMDoSiXivcGmT0eS7c16J7iAGv/qfCHg8Bxb9FSZLoRCdce+2Zq7SnoABSCA7Lb2+JJDdIo8fB2/93VevNhdbu6my5fH969nC+vtmPpnVKLafYgbbpCjweVDBs7KuGopEJYoyFjtHiotH86ZqD7LzS+dDKOnZGoxGVuXOnHMtFBrVguN8eWKLjj8BYEEBTUkXfBUWFaTNFiVIkjIecpLl2NLz9xxXdSHruhaVboY4wY43SwRaC0Rwtj/R706gRAyD7po470m5FLpt+pR/r1pKSI9K/Ez/9Mv75/dfvy9b9f/Rf+NpncBuUfjZUYKC1vvtouqbMZK8/B2LOmWK4AGEqk/hWY6/pg8WcwdilZA9dX02sxcy4wCBNCZ07Y+mXdjltrfoF+QUf6FS1nOtX0hpRVqzIeW6Z2+nC/q7Z8s/hSisOcawDE1L39AQDlwLH832xfSWRQ7QUXhFFCy4oGWNnqTTdPFhAiYtWWqOfHokKAMr2OEwShDmY302Pdl8SXZYDNFkeWe/vBtgqApC4xVuV7k4mR4QIiCSmAlEConPMzFmNIwCitGFGt2vKw2ghJQcskYUGFUk0E4XnI0WWR0dHVkW1fOcgRNcTIYAEFqhY9aO8vZyRG0ilwcSYGSFUBhBWSVEYcJM1NEmqYeMAWFOGFdOQBUJBIGnRccpzNb6aflpsMVAoaDg+VBcMHnWIoZ52iAbkc+hTOzXL+ZR+3uOyWN6mfnRcISAzU01EqoPJOdyPpNgKFBMoCVEYD4hIwyXcH0Wc6FgIVWUFlU1ClAIpShaDdpZxJ2EM9RCEROLyatFyW2++5HQNJ15FTp40H0MTQx1KHKBhtA9MXR6gfevAF7esJvr7rMsH5BCo1tTYHOeqH7XI0K4+0MmkGwnOILfmM5zqefbsadTz7rssHz1ySQuoOJrXcCF4QTgx3qAfAncNzBfwCmBk+iYHeLVtioJLofYB+cj2BUKB7M5nrSPdemBHUETFxCPtniXBoohqYu/uJER2wBfCM6Hbqdpw18l2XEZ4BLBizuJWKguODwdw/j9I5vloaUAGS1T52Aj1RfWzXJuQjsVF8R7Zabe7cgF6i2QAhx72Jm1u5TRyBYYm726GBYfB8hsStMxNamRvmVR55UkaEmuc3J4zI/kSNkDkaJQMfpg8Ii4yUjqC0G5mzI5G/wWNYbSIevTaR22UMSFR4VLWJXCl81Ka50BK8fsB26Cxo9H/g9bJhlfgHKUnkxklAKCdJZuW4YjHaciDUKpdkpYjcYkl8KPuUetXBxZEjCWtbYcxUIse5eeBYfJMVIzr1KNWo4c+OomqPY+YVtcGGrY7LnMraym024Qk1mszDqhD5x01sPgVkAoxSUNhfisiLtFwA1FHQwfVYeHPcFlxGOpljPg2Pe7zGLdAA83O4HaJokRQveNu3cfKKkXREY/jmu6E8ZFSWxPELdTzysyicWmWKdPJgn3iwORpXMBgWAfHLbTxuBHCMzfIE1vmhLukK3BoND4yAp34ayVJoHdwcqO6YWyqJz+BmLxXzjBilDqnoSRlKKiS95fbY9hm86G6tRqkvzIb+XfsM0DhQd9I+A2HGaKlTHnLeZ5BlR0YBuPq3KnR2MJU4qX2K7KCXerdgorpAF8xrFsFrXMKMPZ54ZSRHKKkQ3tHNFq0fKuF9B2RZQfIwvUbaGqSg4Lz62MHwmMqCO2LLPFVsOYThx9sa4gILyhmhBAEMibU1hDgq++RxLDHlkppbQ0gEIkcR5tjQgYAWBBCAJcBUQLM4FASkoEDqj6GwT5BTNYJu09pcC3jBeCr8BNBpdEXftbkeVVwTI7BBCSwotPVZsoIk0md3MvapEY5H0vHKK+b2hD6Hvea9MBN7jVJWNxFE09eGUBYCHZafnjnZtGmHGDchjQ+pNIgn9VmEgPDOGPsgWhcKBGhDH6CQfo1Qf5i9RPtqSa/dmGCd0YTYqjMkrxAXFdIDZ8xxFJ3BvpsQ5L1JpK0fSry/Qbh+w7HnIdQ3FnVOWtI9JtStylSee0z9MoO1UsMUSh26nZWZUmPkwyejUZSa+3WA9dDpaFEOlFwdHlvCu3ezoD1wl1uCMMda5IedFgEOJ+n6VJKzClU7R4wH7ZefXr+7//x68oG+/OvPn7+8ff3+fy8nroAFq+oeNCDO/vtppT+YPGzP1L9QF2B6/2ULFf25+q/b/f9uB7rSb7xbX03vylDeevXwsF5NZw9bCSoBKkispzc3SmcQWG7Ft/+ueqYra7x1wDv6jX/9OL/ZTD5M72aHm/zP8e8F3s85L1fT679vt2o+ud4pdjk9i7vFZrGtrHt8hk6YanBdccjhTcyuxfzqxr7h/HZVzvBUmeaHB97dvfpF049lDOHu6mEbSpiWMrpfL1brkoIQWCmnvpzJD3Pz0o6TmvsU7retjqO6esyvi/lytp/Wh+PzGk2pGrN+q1jtTgvkmBb5NGZHjau7Us57Lfm0vpvMFuv59TZCdBB/0fJwLjkfufThfssE7fMAQsmlG/zUe7ufoN82VvPN/MumuX5P99HYpeKTCztI+3Exm21Laq/n6tfvdynKJXTvR6hR6eUFfVWO9Gmz2j2hI8dh7zY76tN0W4s7lbulsJmzPeHEEX2ryls3Tl3ICLE358qUuGOsMxTacQv7oCTqhvOHTVIpUWIXJXbErWHV2zvBtrZTUPFbX3SbxGyMRJeEzFkPNxCJlMZoHJrVtqIVmLdvpn563GRWJ3a6tTx43NgxHQyiS3v1wo45GgxMDOuFHeunRw5SObHTMRH6EYOHQGEmgpxCPFRayRLpiIda4BmEeILKIT9nrtYEJZBhJDJIcP+GTY7xmK5rPFDsI6iM8hMhEGv5J33juFudNkeDZtn4mARi/3QRu2GTGz8RSlg9N2zqhEohe2dLYkmt0UQyVNo/ne27IHb6hcaXEuE4oHBVlGbI/oPq9ebIozraSMrCytMVBBSuLFFXdd1I/X/cwkocFzk7YSFYWGnQAisJuoTlSuJNKazn81EdKZ8jmwJ7l5YiVWWJ2mgD25hncD5qWIUFwCqOhF2Hc1wlJxJGMQdIeelevTHvPX6vMVHf4/demA/3SGruyMMTnCAJzNEQSJa24oZ0t8j8M6RbTK52SNPcIC0YLaQ060bXIjgkSlnplvtwSGnK+tJOYeBuEb7nSur+fdhW8OeGfWVVWFtaZoPmDu2eLYeeQ5EsG98N6G7hyufjwy0meTud55aFSDAxD7YTfbKtj3ckrWPy2BgtMaRD2gM9+8cN/5iaMuPmxm4HBFijqZU6WQ0hNwKeIySdEwXMbr4M9d+Fk8QebWAEPEdIrAiJpeOOqqkEOdrxpYyQ0AB38qkJygxlMeionjp0KEsHLZ4NxRM9nz3g210fkdki4TIU+zcecRiKhA7rzLP00dnHdgLLu9i2Qzo334diMyuNMcZPOYFFoLRsH9eIiaEtAth6V0LJ3pM9cW3rUVWJcEBrrRi4OYOUkYL4WjlQ6EBJit4NzskOKbDy6Hs3lGVkwwq+BJlTflQH21OkGV/DQ/ZvcGMlIN42VqOAIQUDcatksBZC7AYBbsGM0MEhvBzakR4OQ0psYlS7Q8jeYK+KJg0iMh4hVbIp0vKgoG7sbW9AYktYdbnGKjDRKBZxqErDgWhYhqXGdK1K0xEbrYafq/SEl/UyMfyQ0WoOmiWQgjtMGCXZ05VScitAQI5l6lJK2KE2R1Wh4BA11aFcodsUQv1l1Vc6VGvCzWpNiHXXi7aqS0NpSV5hYdbENu+pIxAaymYGliPlR5vdEbihcQlynPk4lfxCVK6mH8zQD9RDP2KFzobSpLw6dxiapPzdSKo08HojAwyufKIKFHGrefe2LmKEnpAIVoOYdBbNvMUBq/ujDytMBggrVLDu0CbSqBqt/x4vsBDSNye2/+raRxtfOM1tBSRt0STrDemWTEA2wmgtO6MHFnook5EEB7V3NlpgQac/5WvXjbNL5BV4q/Wm9aDVetMXZmK9IWCQvVmpPbidIG0OhOCw5pvo5q5ETF5OhuxGrCwXnIcmN+O8sptNnGPQE+fMGGdgL0UEbOvnEBU7JYeljKTRJswRQS1Ad0fS+sS+EnB+Xge3EDGoum+EGBn9ZbDp+8dqGguN+6AB4l8ywENNs2dW6lbtwt3Lq5/jnieHdlleO6dPGb6yoAMf/ZcRjII+a/cIW2vdBNzKbRr9rdwm8+I254F4hbF+BKdGY9aZqKqw9VCByRFc7LMkIkhMUTHhCF2NQkSjtYfotIXZjXL6MGNsmgo1wWRebndJLGalDQ7ICTQFrdFkMppyd0AJsJdGamyp7BFYgENzO4MlhBSFlFVvO2RzBqegkKL6vi7+XAeGwOqS2sHwCKHwy2//uiNXf5LLz/zyvZz/NkH/PkxL91n2S+3kaYYFxGqiBeZqAWaQATyfODahzKl09HlOMJXOnjEh5VBPWVjZS/F6e3CutTBUdWVr+0AvJsIXS6qkxahkQJbtdLnhX6mVtODUpxBYkEK/PUif0JDqoyP2CaUX5hYhCuoG6oLIhNjNQEfAiNEQFNFm33ajISjSJ3rGbggaUqI0tlIfO3M3tMiUIOrLnGESBEgMCwcdp+sVGqDT0WKZlSw6NEuE0miWKIdLL2oLXfoQ09pWTSOjta2a1qZMzGalZw2A63rvOpC57QporVudw5rYfxfQvEuioGfZrNvzK6B0/oq0TQ4DuPVEL7a7mg6xW9zW5PA0ZUTnqYwKeU2ANvHJ/FoSqoyEeO8Ce+liLIXQjmCqXIqhtEGvdAeNKPNWemy4xVaN0J6+ekHLRDUgkg3QGrWROQIx1iloqIZxF+xfDeOpxv3b//w+f/Hd++9/+O5vePlu9e3tPz852yRGNMODm1F1ygxLZpjryJ6mLYEKV4ESAAvpcJgUlZqFMPsY4U45pV/Sz7NvcShv+cBfpy3fdZmwllSEQWAVFTK6jSgQFxQC/SnjPfsKUIbLypZ6HNzpJolJa4xjsieTVn2T7a5sgpEyKGTIizgL/BeODGQMcWHmmUQjsYCth+FJrKvD3ytlqj+JZUI6ClIFqMX6jR7PBBlbssFOvH9cjrmCI6heRvmMeDzjFEHAbrA+auTYQh//9BGjxCwtw4Wd2K6uK6hjlwCC0vyBEql/BeZmp6Noe2Wdyq/BznR+Nb0WM2c+hFrGCJ0FicqLj2BeRk232MHKO1lUeIcOiqawiFFGxvlEIY5ytb3SIobpw71Cq/rjZvGlFJ2r968lEmVycOBIarnZvgYUldm9T8cQ6mF4VAiASvOHIKjsA9c5Pfcl8bebQ1pGJhXcJZDUJbgdjscTnK7xkqncQmq5R5GbEsWbN0cTw9BeSJlpnXCcjs1IeCFOeVKlY9tXBnIj4Jzk1sl/PGuDQx9p0OY0seRClIHtqLdr9sWNN/khTmA6pZnNb6aflpscVKa57Z+5ygRk6OusnJvl/Mveh7/s5s7rZ+cFAhID9XSUCogl242kc2MLCQij5VYrl4BJzncfa2cfwgJCAiFVHouiSCFoZylm4nTTZhFK0dPHNoaB0BgnsRft7BXItjp41UAQ+++n1S73vPZft/v/rXR26whrtT1cuE16VwpsfXd+u1JfWU4VGexHUU+xG6g5uHr76vDeKevBssxsu5xe/327/VrDBCtfrqWhfG0/qWeBU5unqsZUDqqzQR4raZ8Bq+0r1pkO9SPy2BFGiHFE3g2s9Nn6oVKprusTHmzGKt1sFZp+79XATGhN/R4omAGnaj3snlXPODCcdYapMVpqjnPFsOJzHL8BwMVxXxfz5ewcWI6/AeCsWE5SO146MMulb3pQScVmohY5Vd/sw3uxWS63ZgUM2n2ouGGhd2A5Yuz0lk05jHSDxCyHXFHGbiw33WyUtEv3ymKrEWnpQDdZ0hI0++ZwgB20lKqilBsLIZHLJD1YA2ip+mYGtITyyh5RcEJWoy3Zt7BqucdokZyAwxpfyBXPazem9jsTRw2xio+uV3c3y8X1ZnF3u2etBzdtHTG8DK1Qer9p6oEzq8RBMdP9gahrhcwyl9M6KfVxMZstj8XNmtp2hCb1zQP1KyXpoWZMuTwq69rLcXXdSEZ6AVvVWWcEEGY176aOWR07IwAF2Lx5RugrhDyenAA8+t5yqqSA7sI6r6yAoE7hZ5kVcLLk8k4LCOqIfb5pASdLL+/NMhywv3yeeQHdBXdWiQEh1c4fi9mRX2ZASPXwc8wM6KE055QaEFJa/DGkBqC8wsuPIzcAR0uxn4zkUXPArLqa4JgPZ6NhKI8aZ5Vkf0xYfoyER49Mn9pRXnBkn/qsEu0Ti8tc7hwFb/JZ7sjo0ZAuPvWwksN5S+78cu2HFV+Vm5up/AYLisRwqxOLzuwZn7nossq4TywaYeyH6mcd0bUmZ5R0n1pxmpHE3BXnieTd47wS76GRIQF7FtEwx6mOjA3kXpOA2IyjWea+2IUtu+QH1pkwU1MIs6Nfx51pOIgzTQKiFgMuZ8ek5YdEO2N+bepUPt4zCQhmRLPkARDTWhZaXxc6lZCqUmbNjRaa9cY0GT380cmJTiw7fk6io+PHP7o40KnVTpyT6EZPB+nkQA8rOp615AJCH4/A4NCf2rk5QzvMdNwUjm4O87CKkjfFBQQ6HoO/TPI60UmbFkzviu7+YRI7yzR4L3pwv5hIpUAECMDVvwpPzT4wEw4pKwCvHDKbPzlxCL+sb4xoVXxPRFBKZ1MvFmB3pOmMF1KuM6j5nR8w4RvTWBbb6MQuQMGMMp2QkEIcBCKpnUdQVovmDnc7RvcPt/ACTI+8OvpwxDERHEtMuQxq7+MCkKO7z/j4MZr9YGNphkCUNKDL8lJiG7Wpm/24MTRGwV7McmQAhcnC2HalSuuBw/0QqND+5CCazhN3SRiv86UiCkAumi2C5EXXFkEdMdHaFFMza2tTTL1+ZmLsQWW41mwRYtRMEKRuioC+B94hM+7SuAn23yRxq18REAOL1hErgtr0LXGt1IY11AZ1Vppe/bESqBDOS4WoD9yEGujup0GY+G5CTRUK0qDO3bF6/IZYvbHcqpu+bMEJCpt7r6xwxcTnqZhY+vDKYBTFJH6lYH0UM1oQJPiE+/BBEM6MpDcmHaFiqOaPOjoK+BIFzP4l0WxqZ10w53Se5IYcO2EUPsvmBA6TXeGes/EP/weJwi/wdo9wlHwK54921nUyF8WI+RQveuZTDCKWKqGiIIwSSjlhmDHjRIksIEQ1ksllU8Qt3tH3fVuzLYaULMMFRBJSACmBiuzEGUs2IDg7cjLGoDqr7DOqph0KKpQCI6Oz9nmJdvyz6AkMkuNKWahlT5sWEIOmpUfd5towCQJu8eR+WH1I4UFcUHFQO9bcnqSyYBhV1o2jbHROavdETrPTvE6zE1JIfiBm2rS2OC0krqxjoFNQujfiNW6Cu9wlteMd4PXklKqv1lGz7mPVBDADb9w5xyzAzwmsNtAp56qS7bn55+5Z7JTOMZRl0ls4FSg6FKdsrm/ZnY8IydnI1J8fUIxQB5rOInk7JIciPyd+QHGys5LmuAdyezruQyonPydpBjiDZ7QiHjVXPFQ6fso+C/DccvPIBxXRGaXxs4BU7Efhhe+lkIkXbh4i7ZvHD1vOAyR2pVkHVzrPonIUGkkBSiMcyeBjF5XjAf50pmVdWGcCzb+oHB99ny9ZUbnu4jqronJ8MId98KJyJ0su76JyfDC3fJyicieLL/MCS3x0RzxZUbnuojuvonJ8fK97ONFkWFSO5+5wD6g46KwU54l41ywz7/qRFJXjrpzmRrs9uxFo+cHkYSvyF+oCTO+/XPiaIFdN9d6tr6Zlr9Dr9erhYb2azh628lLiQmCznt7cKBAisNwKy9OT72od8I5+41+/lMNNPkzvZoe7/M/xLwbe0DkxV1UHvsm+SWE5P4u7xWYxXXqn6IS5dvdDZNdifuVoLj2/XV20tZaefiz5/O7qYUvr01JI9+vFal0ebkFgpZi0nMkPc/PSjpOa+xRKxvGUeWBdPWa9Y/fD8XmNplWNWb9VJHanBXJMjXwqs2PC1V0p572WfFrfTQ7BtIP4i079yL2XPtxvqaB9HkAou3SDn3pv9xOCG3vqhp3L+U35ydF2neu5+vX78GC5du6P1alR6eUFfVWO9Gmz2j1hp46h9koZqz8npdLsz0movQvEnP05ZTKb1BX67dahOqR1bPm66UmVGbS7Ll/bT7Jsd02gzl+pgt9Yh1nq7a6xI/KdrN2189zoc4/h8+4xTKBRu2DCgbAd62F7DOsfkJjCjpkqdcMkaxIrH+AF64CjsUlMotFJ7Hij9FBoTTcbNVc7E8+AyIhYOMg4TywI01QC0IEFV12vdFgICXyfVggh1MyorqsKj1yE1z9oFllwR4d8gqyXMvCalJlEkbbUYtT/Y5j0zNQoT9VLZoxGzMhj4oCSCDlLdRoUq8XChksoZfQBZ2QoatMgHygisxQlE6xnWFONRplhjjFm9rVMDcWQPY3ToFgB6ryhmFfdFgUeDE3wSL1D1QOKwtzSYdysjxoPih9/WU2/uXr7xw9/ss+zxa+E8c3DJPis0ml2S5/sNGxuFDMBeCGl1IUpXQVoSYFZVbmSOEwdDlnBxKEsTqpqlCHNJ06pRsleiteXby7sftHRalF68RKeqEGhEol5Gl7XIOO44OggUIdXDGgBHaf5ktWnDOk8kVcl2qDisy64TCCOWn42DmAgxgWoHaA1WAACUlBwgIxeuseuPhvS9iK2vu+z+XMRYCUhWpcQkLYTisWgKp3eCa2k0zS3iC2InKrv+eTdWnxPA761+J6+MBMjTlCDUJhwwrWrOSeNaiKKmrzjJq4eG5Q3dULB2COI71RnuVA2WBPRULZCWv1llX+N4qmcpA0kr/gNA82TjYyyJhj7YVwNWwACIAdMOc1AE6vOoaZq3eVKv6AkXP3/YfEeUAMxWrXkE8Bfq3osGnCFonux8N61l1vqJftU4dz4vsQsp0xQobxDwZlwYJYjxsqSbRLxniFObKTOQgL6sH/XysfUKI+iPFrXbWMVO3biAp240Ng6ZAPSF2zIBGcTyDE2dmAop0ZkMTw4xJC5n2OPFglF6qcDsyWuutkA2Ak51/JUsUOlMU4H7Ehkjma5V9Gw47iZztpKip1uXt0jxo6DKaiZ+9YBO9zMKaDI3G2Jhx37ZlQov4mkh0+37cAQ+MymDx8q89I+PPem/OeMUSaIzSn9GcoezcJs4q0PGICAkfY+mCS0kJJRiEt6NbacKOei3AfhTFJwqJBXBwKEBabV15EjGEoQLBCqxsAxAqPuaQ4IRIwXUGeIC4i58mmVyQyxFV2HQM0MBVxyziiRZqidcTtS6+wTRkIDtTbY/OAN7wnGGpDgvLmnjnmBMEaUYUy5ANhRZFFnpzaKQHCZCjcB58dit3MbXUgQmVKyhFTTWVc7t0SZfu7nC4i5jNdyUxHBbDE/5DYczcCLJLuJUHxBMBFMYMAFR814MCLbvU6P8JAcVHgBG9QJOopF3xJRK6ERRC47UnaJfzljyh1hUo+KeXUlE3uOybp9oUWvt+VbbIPgcLHvJhALF5kNZPyFNCNJ3VzPpQon7/T1wG59c8OL3XaQ8/MBOSy7JNeMaNhz3w9C310E894kkiPtfVDufM60IeHR+t95lpcuu4N9N1aiax4/T80jSrmUO0UELRtulBuCTVsIKCsXUYK5kFxSKXpm89Im7o1Sy1CouxwWGJGuyZ1bCYILBA4eX+AYNR2LSdmAyjJIISCOAiIl2XgAcZJRigOW5egexS6LynFc5EgVLD1GP6ewgkW33m3HRXFoJqELwsQXSyI+r7N28Dz3tZE6yqeVcjVWM6FcVhBCJCi3gDkUghjhHVqA+qunRV/GJRFFGHIoBWekyz0S0y1G+dJtlWZ+oFsB1WRhgBjGgiCbeymQtYAcl7qEXLPMJ3AMEl/7A3ZpH30El0aODVZwDY4vIQmLWlq0JM34Eubu+O54EVwdQx5yMQ+ODqYTEwU+MSFRyumgs9BWfSxcS32qOCDuVOAwqsk1jpzq+lLJgVaL2XiCSH8qcKiYbBLDTIu41WnGoTFZfWEmFhwWSDnNTCrzTflnUDZthtKZpaLsbIiRWvJM1g5OVGwMygrMDjrAZFieRzSTLWCL71gQNuOY0dnAn+QFf9p0WSBrWrdmvYBQxBNgjAv940aKyRJj4efOu6bNrgzYnx1epQqOGlqFMPHrlSPxPbrO5HXQnGADsjKSKhhRAb8DmZr9842PMtHcUWlMmnLeAfLnXxHlvNedMYfbhYVsbH9G6J7izpLN+jzzuaZf4fAgrjv9Sh91d2dfEVfbl2GddxIQ8s08/aq7kMz0q+5SQoO6jAQNL6ShErC6S2/CcMFqCVjGkRZlgjUTTuyWL0hnow8jvVNPv5ylt98rVUWDodWQ07TV6vzoC3Ox+ABxWQza94cG8fQz/zD13QSbN0nWT88tkPRVGvo4KyUlNLwVwZN5K8Egz2vnURLfomnjqh94GfcuzX5zOjV2A2LoY2QT9opiDUHPmSBXgckHKob8jNghD9Zzl9Lo9ClIpHgUBH2e9dgjCe9g3BzshOkRztlJe7I0YCdGt3Z1JBI1bWijEmPtkxm7YpSFuDfpz5vprJcqxsGYvTHJiKPzV8p0MJIogOnNO6oENmzekZ7bTKgRS+UKq8UUEiwglbBZvhf3rQSMSFmbTflmaghRVqgwUohIISmxar8NtYx37esR2hrn18kuQIbAX5/udO+az+o3lo08enTG+deP85ujraMufEXSM+weY1bMOZH+fHlYVPvyFc1J5Dg9yxxqBUmqMICzcuBzN5mz6ibDEDUXUCId0cFUjRjcwAqIDj614vteDcxk6VVwwkZptQnFOgG0T60FbtIeSVfx3D3DcXuCKMFP86ClvHuCMDPGp+x67YI3Dh4n6gnixkL6QF+oVDKnpbySEspVznADJpQiAyYdaMlM+FHgRMPG7Wj8IkDdMJCNaCkBop62bFeD6738EOXpdRo6WnUpChnz33mAJCx6aiHeJ4OxEwreETWab+h01e/a7jxIKTwakEj7NDDWRjYnFMZTNjPwYixZlby2Ow9WMq9bOPa5Zh6RsIWWTmA8/9BDV9Nz9rTKMnb6S3nlc/C0qy9JGfYCLoNIKgQBSYujtbPi1hbb1nJQPg9inCBgp6OR7d6n+hArQ/ZwOqeRRkxkIWg1Bje3qKPNrZbkk84jjn0I+ADY8OOlTJYp+/skR7PpSPMQMNafjpdGzAI0MvczwD2khJWnA2qVOVBDSkrXSzFZSlsPyaEh84h12muOyd4nlnHsILuGDvmEg13LWjrhZJcm3O9U5MiFGkFo2FUrQ2sKsb4wE3eD0cbCgJoGGgfOlaFzCjERnuWn5SaJXRB9gj+3LMxBazomgHleXjUmrD8Cg2EufVYWRL1w3vkEMej+pEnjP6zbdknGR/QTlXUMV77QYyqZKR9hPkiWVR9jaB9hvkWGjbrIKKO+XQlGizEg6t3v4rJb0TFHthJVvouo1S9KVXKQP7V4g6vBb/x4Q+d2zMhoUZ9dhIEnjjCc1Gg9oVx00pqOKRh1xcauZsUThxRO7IedTjATu2cHtkMIZNAQAu8WQhimAzYoENeVX/qfrUxu0jk6AHsXrFaTTmtGJibdIFXH1FrZMExg0zShzeo2A5885kPGDXroSR8HZ7It7NNUCYJEa8jBEWDrGYcI1ptQVyg3vUlVrow2VaVpftF+qnKGxcv4qbmZ0RUx2+oYKZQts7hDooJoFPqUDRLap1JgtHSrfCMNHFuZ4PtshsozvLCDC6R+khtihztSFk4TnuBONIsYBVjEz+GFi27eEu3uxQqHs5RTdAEF2Ia5Rxd6iEU2tVs5CW1yGbbwGUpctNwllmESFjoIK1hnqlJnw8jm1AMPT8dwo6F2m6ahVrtNX5iL3WbUGzfKnPXfIeLNcY3KZkNvCqFu5y8Gi691rV3WF/v9a56FawAO1YC8ivojA6kyjgJgU7HGxX/AwZCziaANweOZgJPoWmPa4cWwYPgAI9a3yQRvxneb1bQxDis03j1oZTwNcD7N0TUFN78uun0bhjxy0oiZcgsdavhcSeasKsmQHCvJQJioAeSjKyVT6WAmDL89ERuxlgwZv5YM1AUin4vJDMxM2Ir5jl5MBsL0qQ2Po5pMpTYZMZPuK3w4tdy/nAzh1ByNyWSJBPdv//P7/MV377//4bu/4eW71be3//w0ybuT4pG45Gyxnl/vR7tbrUvJp6xIxQBWhjEBWAJMBWx6sRNG+La1jv7YcfiKg8LVTgeqYflxDIaSjFOuWTVm7JnvdJovDE/iI5+y1OM5vusyYS3IyxxpAKot4+a+NKecF/Twad90KS4BLQjsd5fELBcQ63zqLEekwAokFcsZBjgjFObHclk1C3wsLMcDWY7nxXISlpW4D6+m4c+ZYqcILCcphgUB/e6SmOUCmhM+sxygfpYD+bGcM0b6THMn0pwMpDmZGc0hiLw0x6LQHFMmo4/mfHeJR3Nf4V+//vD5y9uPn6/mP/6yevHn5Qs+CYjr5kdzR6gqFM/evEYs6pRmMBqXtIXRCFCWnSvbRjkNEXLUnDJ0hUNHZ7QeKQTdU3VsRjOYpR/F+TSlTnHHrxuN0QhQbiMtGYcRKIyuPpAqp1IA/SnjRsHK8C4ulNaDOMw4NNR2n8ScFhARfua0A6fBHDktfY34Z06rNKWV03C+nIbwMJzWdp/EnHb2Wwuf5w+bpDTHqLLlj9McwzhDmstyZ+HR0Zxjq+H4dWPRnGTMQ3MY8gb99GQ5uj3IWd0Ed7lHYoY7+22FsRmOUpYhw2W5q/DoGM6xzXD8ukwZTpL0DOe/R2KGO/stheQMRyhvuKrIoDgO2rJDxqC4PLcUHh3HOfYYjl83FschCRreapN/EBExOG57lP4oybXcJPWGwlnuKNRZbj5NzHJ0V9j1KMsxSHJkOfTMckNsMwTvM4x7rBFwdJznMGYxeA4yzjzGnP8mqXnuLHcZsuI5inN0WOHz1sMgPBe69wBH3Xzw8xyBUXxWP8+13CQ1z7l2Hp4PW6XetGUQGOl0XMGkcDQJQ7pEfOzzVm44pN+xqCbcPhIVKqoj+XEM+wnwaBmLhDQ46p7DFmfGcWMOe2+hQkaJiVponuhKzVeufYRnvkrNVwQyC0c6lXE8skq/+fC0yGrU7YMSZNyiFwYK3aGgz2l2s0eG4itjtN58pf5cr0piOVxeFoH8aTWbl1f8Pw==</diagram></mxfile>" > @@ -23,7 +23,7 @@ pointer-events="none" /> - + - + - - + + - + - + - - - + + + @@ -139,7 +139,7 @@ - + @@ -176,9 +176,9 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - + - + - + - + - - - + + + - + - + - + - - + + - - + + - + - + - - - + + + - + - + - + + + + + + + @@ -589,16 +613,16 @@
- attention area + attention lane
- attention area + attention lane - - + + @@ -654,7 +678,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + - + - - + + - attention area + attention lane - attention area + attention lane - - + + @@ -875,7 +899,7 @@ stroke-miterlimit="10" pointer-events="none" /> - + @@ -990,10 +1014,10 @@ stroke-miterlimit="10" pointer-events="none" /> - - + + - - + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg new file mode 100644 index 0000000000000..2fc22c8a4a401 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg @@ -0,0 +1,518 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + default stop +
+ line + +
+ + +
+
+
+ + default stop... + + + + + + + + +
+
+
+ + occlusion + +
+
+
+ + occlusion + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + first attention +
+ stop line + +
+ + +
+
+
+ + first attentio... + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index 8802dc786b617..c29cb7ade21cd 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -8,7 +8,7 @@ width="2842px" height="1201px" viewBox="-0.5 -0.5 2842 1201" - content="<mxfile host="Electron" modified="2023-08-28T08:00:31.600Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="CbgId97oWJwBUVWni98L" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + content="<mxfile host="Electron" modified="2023-10-03T03:33:38.127Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="TqIhwSEqRvc68Imy-9XT" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > @@ -18,7 +18,7 @@ - + @@ -57,9 +57,9 @@ stroke-miterlimit="10" pointer-events="all" /> - + - + - - - - + + + + - - - + + + - + - +
- +
autoware_perception_msgs behavior_velocity_planner_common geometry_msgs - grid_map_core interpolation lanelet2_extension libopencv-dev - magic_enum motion_utils nav_msgs pluginlib @@ -41,8 +39,6 @@ vehicle_info_util visualization_msgs - grid_map_rviz_plugin - ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index fbb82d6ace175..c875611b16003 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -271,29 +271,37 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() { - // TODO(Mamoru Sobue): collision stop pose depends on before/after occlusion clearance motion_utils::VirtualWalls virtual_walls; motion_utils::VirtualWall wall; - wall.style = motion_utils::VirtualWallType::stop; if (debug_data_.collision_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.collision_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_first_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection"; wall.ns = "intersection_occlusion_first_stop" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_first_stop_wall_pose.value(); virtual_walls.push_back(wall); } if (debug_data_.occlusion_stop_wall_pose) { + wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_stop_wall_pose.value(); virtual_walls.push_back(wall); } + if (debug_data_.absence_traffic_light_creep_wall) { + wall.style = motion_utils::VirtualWallType::slowdown; + wall.text = "intersection_occlusion"; + wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; + wall.pose = debug_data_.absence_traffic_light_creep_wall.value(); + virtual_walls.push_back(wall); + } return virtual_walls; } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index e02a1e4220030..d42c9b3ca2aa2 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -136,6 +136,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); ip.occlusion.temporal_stop_before_attention_area = getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); + ip.occlusion.absence_traffic_light.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); + ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( + node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); } void IntersectionModuleManager::launchNewModules( @@ -165,13 +169,21 @@ void IntersectionModuleManager::launchNewModules( continue; } - const auto associative_ids = - planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); const std::string location = ll.attributeOr("location", "else"); const bool is_private_area = (location.compare("private") == 0); + const auto associative_ids = + planning_utils::getAssociativeIntersectionLanelets(ll, lanelet_map, routing_graph); + bool has_traffic_light = false; + if (const auto tl_reg_elems = ll.regulatoryElementsAs(); + tl_reg_elems.size() != 0) { + const auto tl_reg_elem = tl_reg_elems.front(); + const auto stop_line_opt = tl_reg_elem->stopLine(); + if (!!stop_line_opt) has_traffic_light = true; + } const auto new_module = std::make_shared( - module_id, lane_id, planner_data_, intersection_param_, associative_ids, is_private_area, - enable_occlusion_detection, node_, logger_.get_child("intersection_module"), clock_); + module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, + has_traffic_light, enable_occlusion_detection, is_private_area, node_, + logger_.get_child("intersection_module"), clock_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 89bb65f403095..89cdeaae56723 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -17,7 +17,6 @@ #include "util.hpp" #include -#include #include #include #include @@ -65,14 +64,18 @@ static bool isTargetCollisionVehicleType( } IntersectionModule::IntersectionModule( - const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, + const int64_t module_id, const int64_t lane_id, + [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), node_(node), lane_id_(lane_id), associative_ids_(associative_ids), + turn_direction_(turn_direction), + has_traffic_light_(has_traffic_light), enable_occlusion_detection_(enable_occlusion_detection), occlusion_attention_divisions_(std::nullopt), is_private_area_(is_private_area), @@ -81,11 +84,10 @@ IntersectionModule::IntersectionModule( velocity_factor_.init(VelocityFactor::INTERSECTION); planner_param_ = planner_param; - const auto & assigned_lanelet = - planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); - turn_direction_ = assigned_lanelet.attributeOr("turn_direction", "else"); - collision_state_machine_.setMarginTime( - planner_param_.collision_detection.state_transit_margin_time); + { + collision_state_machine_.setMarginTime( + planner_param_.collision_detection.state_transit_margin_time); + } { before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); before_creep_state_machine_.setState(StateMachine::State::STOP); @@ -226,6 +228,23 @@ void prepareRTCByDecisionResult( return; } +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::OccludedAbsenceTrafficLight & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); + const auto closest_idx = result.closest_idx; + const auto collision_stop_line_idx = result.closest_idx; + *default_safety = !result.collision_detected; + *default_distance = + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + *occlusion_safety = result.is_actually_occlusion_cleared; + *occlusion_distance = 0; + return; +} + template <> void prepareRTCByDecisionResult( const IntersectionModule::OccludedCollisionStop & result, @@ -570,6 +589,59 @@ void reactRTCApprovalByDecisionResult( return; } +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + const IntersectionModule::OccludedAbsenceTrafficLight & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "OccludedAbsenceTrafficLight, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + if (!rtc_default_approved) { + const auto stop_line_idx = decision_result.closest_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { + const auto stop_line_idx = decision_result.first_attention_area_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->occlusion_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(decision_result.closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { + const auto closest_idx = decision_result.closest_idx; + const auto peeking_limit_line = decision_result.peeking_limit_line_idx; + for (auto i = closest_idx; i <= peeking_limit_line; ++i) { + planning_utils::setVelocityFromIndex( + i, planner_param.occlusion.absence_traffic_light.creep_velocity, path); + } + debug_data->absence_traffic_light_creep_wall = + planning_utils::getAheadPose(closest_idx, baselink2front, *path); + } + return; +} + template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, @@ -675,7 +747,8 @@ void reactRTCApproval( static std::string formatDecisionResult(const IntersectionModule::DecisionResult & decision_result) { if (std::holds_alternative(decision_result)) { - return "Indecisive"; + const auto indecisive = std::get(decision_result); + return "Indecisive because " + indecisive.error; } if (std::holds_alternative(decision_result)) { return "Safe"; @@ -695,6 +768,9 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult if (std::holds_alternative(decision_result)) { return "OccludedCollisionStop"; } + if (std::holds_alternative(decision_result)) { + return "OccludedAbsenceTrafficLight"; + } if (std::holds_alternative(decision_result)) { return "TrafficLightArrowSolidOn"; } @@ -747,15 +823,15 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - RCLCPP_DEBUG(logger_, "splineInterpolate failed"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"splineInterpolate failed"}; } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{ + "Path has no interval on intersection lane " + std::to_string(lane_id_)}; } + // cache intersection lane information because it is invariant const auto & current_pose = planner_data_->current_odometry->pose; const auto lanelets_on_path = planning_utils::getLaneletsOnPath(*path, lanelet_map_ptr, current_pose); @@ -766,30 +842,40 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.occlusion.occlusion_attention_area_length, planner_param_.common.consider_wrong_direction_vehicle); } + auto & intersection_lanelets = intersection_lanelets_.value(); + + // at the very first time of registration of this module, the path may not be conflicting with the + // attention area, so update() is called to update the internal data as well as traffic light info const auto traffic_prioritized_level = util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets_.value().update(is_prioritized, interpolated_path_info); + intersection_lanelets.update(is_prioritized, interpolated_path_info); - const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); - const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); + // this is abnormal + const auto & conflicting_lanelets = intersection_lanelets.conflicting(); + const auto & first_conflicting_area_opt = intersection_lanelets.first_conflicting_area(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt) { - RCLCPP_DEBUG(logger_, "conflicting area is empty"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"conflicting area is empty"}; } const auto first_conflicting_area = first_conflicting_area_opt.value(); - const auto & first_attention_area_opt = intersection_lanelets_.value().first_attention_area(); + // generate all stop line candidates + // see the doc for struct IntersectionStopLines + const auto & first_attention_area_opt = intersection_lanelets.first_attention_area(); + /// even if the attention area is null, stuck vehicle stop line needs to be generated from + /// conflicting lanes const auto & dummy_first_attention_area = first_attention_area_opt ? first_attention_area_opt.value() : first_conflicting_area; + const double peeking_offset = + has_traffic_light_ ? planner_param_.occlusion.peeking_offset + : planner_param_.occlusion.absence_traffic_light.maximum_peeking_distance; const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, - planner_param_.occlusion.peeking_offset, path); + peeking_offset, path); if (!intersection_stop_lines_opt) { - RCLCPP_DEBUG(logger_, "failed to generate intersection_stop_lines"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto @@ -797,38 +883,49 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; - const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); + // see the doc for struct PathLanelets + const auto & conflicting_area = intersection_lanelets.conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( lanelets_on_path, interpolated_path_info, associative_ids_, first_conflicting_area, - conflicting_area, first_attention_area_opt, intersection_lanelets_.value().attention_area(), - closest_idx, planner_data_->vehicle_info_.vehicle_width_m); + conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx, + planner_data_->vehicle_info_.vehicle_width_m); if (!path_lanelets_opt.has_value()) { - RCLCPP_DEBUG(logger_, "failed to generate PathLanelets"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"failed to generate PathLanelets"}; } const auto path_lanelets = path_lanelets_opt.value(); - const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); + // utility functions + auto fromEgoDist = [&](const size_t index) { + return motion_utils::calcSignedArcLength(path->points, closest_idx, index); + }; + auto stoppedForDuration = + [&](const size_t pos, const double duration, StateMachine & state_machine) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stopline = (dist_stopline < 0.0); + const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + state_machine.setState(StateMachine::State::GO); + } else if (is_stopped_duration && approached_dist_stopline) { + state_machine.setState(StateMachine::State::GO); + } + return state_machine.getState() == StateMachine::State::GO; + }; + // stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); if (stuck_detected && stuck_stop_line_idx_opt) { auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - const double dist_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(stuck_stop_line_idx).point.pose.position); - const bool approached_stop_line = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool is_stopped = planner_data_->isVehicleStopped(); - if (is_stopped && approached_stop_line) { - stuck_private_area_timeout_.setStateWithMarginTime( - StateMachine::State::GO, logger_.get_child("stuck_private_area_timeout"), *clock_); - } - const bool timeout = - (is_private_area_ && stuck_private_area_timeout_.getState() == StateMachine::State::GO); + const bool stopped_at_stuck_line = stoppedForDuration( + stuck_stop_line_idx, planner_param_.stuck_vehicle.timeout_private_area, + stuck_private_area_timeout_); + const bool timeout = (is_private_area_ && stopped_at_stuck_line); if (!timeout) { if ( default_stop_line_idx_opt && - motion_utils::calcSignedArcLength(path->points, stuck_stop_line_idx, closest_idx) > - planner_param_.common.stop_overshoot_margin) { + fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { stuck_stop_line_idx = default_stop_line_idx_opt.value(); } return IntersectionModule::StuckStop{ @@ -836,15 +933,16 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } + // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { - RCLCPP_DEBUG(logger_, "attention area is empty"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"attention area is empty"}; } const auto first_attention_area = first_attention_area_opt.value(); + // if attention area is not null but default stop line is not available, ego/backward-path has + // already passed the stop line if (!default_stop_line_idx_opt) { - RCLCPP_DEBUG(logger_, "default stop line is null"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"default stop line is null"}; } const auto default_stop_line_idx = default_stop_line_idx_opt.value(); @@ -873,25 +971,25 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // is_go_out_: previous RTC approval // activated_: current RTC approval is_permanent_go_ = true; - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } + // occlusion stop line is generated from the intersection of ego footprint along the path with the + // attention area, so if this is null, eog has already passed the intersection if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { - RCLCPP_DEBUG(logger_, "occlusion stop line is null"); - return IntersectionModule::Indecisive{}; + return IntersectionModule::Indecisive{"occlusion stop line is null"}; } const auto collision_stop_line_idx = is_over_default_stop_line ? closest_idx : default_stop_line_idx; const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); - const auto & attention_lanelets = intersection_lanelets_.value().attention(); - const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent(); - const auto & occlusion_attention_lanelets = intersection_lanelets_.value().occlusion_attention(); - const auto & occlusion_attention_area = intersection_lanelets_.value().occlusion_attention_area(); - debug_data_.attention_area = intersection_lanelets_.value().attention_area(); - debug_data_.adjacent_area = intersection_lanelets_.value().adjacent_area(); + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); + const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); + const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); + debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area const auto intersection_area = planner_param_.common.use_intersection_area @@ -902,17 +1000,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - // calculate dynamic collision around detection area - const double time_delay = (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.state_transit_margin_time - - collision_state_machine_.getDuration()); + // calculate dynamic collision around attention area + const double time_to_restart = (is_go_out_ || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.state_transit_margin_time - + collision_state_machine_.getDuration()); const auto target_objects = filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( *path, target_objects, path_lanelets, closest_idx, - std::min(occlusion_stop_line_idx, path->points.size() - 1), time_delay, + std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, @@ -959,74 +1057,63 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // check safety + // distinguish if ego detected occlusion or RTC detects occlusion const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); - if ( - occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || - ext_occlusion_requested) { - const double dist_default_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(default_stop_line_idx).point.pose.position); - const bool approached_default_stop_line = - (std::fabs(dist_default_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_default_stop_line = (dist_default_stopline < 0.0); - const bool is_stopped_at_default = - planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (over_default_stop_line) { - before_creep_state_machine_.setState(StateMachine::State::GO); - } - if (before_creep_state_machine_.getState() == StateMachine::State::GO) { - const double dist_first_attention_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(closest_idx).point.pose.position, - path->points.at(first_attention_stop_line_idx).point.pose.position); - const bool approached_first_attention_stop_line = - (std::fabs(dist_first_attention_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_first_attention_stop_line = (dist_first_attention_stopline < 0.0); - const bool is_stopped_at_first_attention = - planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (planner_param_.occlusion.temporal_stop_before_attention_area) { - if (over_first_attention_stop_line) { - temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); - } - if (is_stopped_at_first_attention && approached_first_attention_stop_line) { - temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); - } - } - const bool temporal_stop_before_attention_required = - planner_param_.occlusion.temporal_stop_before_attention_area && - temporal_stop_before_attention_state_machine_.getState() == StateMachine::State::STOP; - if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx}; - } else { - return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx}; + const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + // Safe + if (!is_occlusion_state && !has_collision_with_margin) { + return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Only collision + if (!is_occlusion_state && has_collision_with_margin) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // Occluded + const bool stopped_at_default_line = stoppedForDuration( + default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + before_creep_state_machine_); + if (stopped_at_default_line) { + // in this case ego will temporarily stop before entering attention area + const bool temporal_stop_before_attention_required = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? !stoppedForDuration( + first_attention_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + temporal_stop_before_attention_state_machine_) + : false; + if (!has_traffic_light_) { + if (fromEgoDist(first_attention_stop_line_idx) <= -peeking_offset) { + return IntersectionModule::Indecisive{ + "already passed maximum peeking line in the absence of traffic light"}; } + return IntersectionModule::OccludedAbsenceTrafficLight{ + is_occlusion_cleared_with_margin, has_collision_with_margin, + temporal_stop_before_attention_required, closest_idx, + first_attention_stop_line_idx, occlusion_stop_line_idx}; + } + if (has_collision_with_margin) { + return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } else { - if (is_stopped_at_default && approached_default_stop_line) { - // start waiting at the first stop line - before_creep_state_machine_.setState(StateMachine::State::GO); - } - const auto occlusion_stop_line = planner_param_.occlusion.temporal_stop_before_attention_area - ? first_attention_stop_line_idx - : occlusion_stop_line_idx; - return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; + return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } - } else if (has_collision_with_margin) { - return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } else { + const auto occlusion_stop_line = + (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) + ? first_attention_stop_line_idx + : occlusion_stop_line_idx; + return IntersectionModule::FirstWaitBeforeOcclusion{ + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; } - - return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } bool IntersectionModule::checkStuckVehicle( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 550b23aa17606..7bd923ed6198c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -18,9 +18,7 @@ #include "util_type.hpp" #include -#include #include -#include #include #include @@ -116,10 +114,18 @@ class IntersectionModule : public SceneModuleInterface double ignore_parked_vehicle_speed_threshold; double stop_release_margin_time; bool temporal_stop_before_attention_area; + struct AbsenceTrafficLight + { + double creep_velocity; + double maximum_peeking_distance; + } absence_traffic_light; } occlusion; }; - using Indecisive = std::monostate; + struct Indecisive + { + std::string error; + }; struct StuckStop { size_t closest_idx{0}; @@ -159,6 +165,15 @@ class IntersectionModule : public SceneModuleInterface size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; + struct OccludedAbsenceTrafficLight + { + bool is_actually_occlusion_cleared{false}; + bool collision_detected{false}; + bool temporal_stop_before_attention_required{false}; + size_t closest_idx{0}; + size_t first_attention_area_stop_line_idx{0}; + size_t peeking_limit_line_idx{0}; + }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. @@ -174,20 +189,22 @@ class IntersectionModule : public SceneModuleInterface size_t occlusion_stop_line_idx{0}; }; using DecisionResult = std::variant< - Indecisive, // internal process error, or over the pass judge line - StuckStop, // detected stuck vehicle - NonOccludedCollisionStop, // detected collision while FOV is clear - FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion - PeekingTowardOcclusion, // peeking into occlusion while collision is not detected - OccludedCollisionStop, // occlusion and collision are both detected - Safe, // judge as safe - TrafficLightArrowSolidOn // only detect vehicles violating traffic rules + Indecisive, // internal process error, or over the pass judge line + StuckStop, // detected stuck vehicle + NonOccludedCollisionStop, // detected collision while FOV is clear + FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion + PeekingTowardOcclusion, // peeking into occlusion while collision is not detected + OccludedCollisionStop, // occlusion and collision are both detected + OccludedAbsenceTrafficLight, // occlusion is detected in the absence of traffic light + Safe, // judge as safe + TrafficLightArrowSolidOn // only detect vehicles violating traffic rules >; IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const bool is_private_area, const bool enable_occlusion_detection, rclcpp::Node & node, + const std::string & turn_direction, const bool has_traffic_light, + const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** @@ -211,7 +228,8 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Node & node_; const int64_t lane_id_; const std::set associative_ids_; - std::string turn_direction_; + const std::string turn_direction_; + const bool has_traffic_light_; bool is_go_out_ = false; bool is_permanent_go_ = false; @@ -230,6 +248,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; + // NOTE: uuid_ is base member // for stuck vehicle detection diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 37dc83cbdf761..f381925b13208 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -16,10 +16,10 @@ #include "util_type.hpp" +#include #include #include #include -#include #include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 7ba894bd9d32a..6acc1d60443bf 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,14 +20,6 @@ #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 7e5bff89485ed..5c188f4aebebf 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -51,6 +51,7 @@ struct DebugData std::optional> nearest_occlusion_projection{std::nullopt}; autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + std::optional absence_traffic_light_creep_wall{std::nullopt}; }; struct InterpolatedPathInfo From a9bb25adda3f33adefaf5cccbc3402307a0f2e52 Mon Sep 17 00:00:00 2001 From: Felix F Xu <84662027+felixf4xu@users.noreply.github.com> Date: Wed, 4 Oct 2023 12:18:37 +0800 Subject: [PATCH 026/206] build(lidar_apollo_segmentation_tvm): fix compiling error tier4_autoware_utils (#4516) * fix compiling error tier4_autoware_utils:: Signed-off-by: Felix F Xu * style(pre-commit): autofix --------- Signed-off-by: Felix F Xu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> From 26e3be8569945733f2e76ba59a7077ccb0c29fab Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 4 Oct 2023 14:14:34 +0900 Subject: [PATCH 027/206] fix(obstacle_cruise_planner): fix coordinate transformation bag (#5180) * fix coordinate transformation. linear.x denotes longtidional velocity, not the x velocity in the world coordinate Signed-off-by: Yuki Takagi * declare Eigen type Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi --- planning/obstacle_cruise_planner/src/node.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index b5e0718f20a49..7de6bb2c966f6 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -100,14 +100,15 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & traj_points, const Obstacle & obstacle) { const size_t object_idx = motion_utils::findNearestIndex(traj_points, obstacle.pose.position); - - const double object_vel_norm = std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y); - const double object_vel_yaw = std::atan2(obstacle.twist.linear.y, obstacle.twist.linear.x); const double traj_yaw = tf2::getYaw(traj_points.at(object_idx).pose.orientation); - return std::make_pair( - object_vel_norm * std::cos(object_vel_yaw - traj_yaw), - object_vel_norm * std::sin(object_vel_yaw - traj_yaw)); + const double obstacle_yaw = tf2::getYaw(obstacle.pose.orientation); + + const Eigen::Rotation2Dd R_ego_to_obstacle(obstacle_yaw - traj_yaw); + const Eigen::Vector2d obstacle_velocity(obstacle.twist.linear.x, obstacle.twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], projected_velocity[1]); } double calcObstacleMaxLength(const Shape & shape) From 9cf1f7cc4f8255dce34273eb8aba4ec59a1797a7 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Wed, 4 Oct 2023 15:14:03 +0900 Subject: [PATCH 028/206] perf(ndt_scan_matcher): changed default `initial_estimate_particles_num` to 200 (#5220) Changed initial_estimate_particles_num to 200 Signed-off-by: Shintaro Sakoda --- .../ndt_scan_matcher/config/ndt_scan_matcher.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 718bc6ca3b3a3..cf41a4cf55d0b 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -41,7 +41,7 @@ converged_param_nearest_voxel_transformation_likelihood: 2.3 # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 + initial_estimate_particles_num: 200 # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] lidar_topic_timeout_sec: 1.0 From bc6806390ffbe07b91d23e2a7ceea641280aa03a Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 4 Oct 2023 15:53:57 +0900 Subject: [PATCH 029/206] feat(yabloc_image_processing): support both of raw and compressed image input (#5209) * add raw image subscriber Signed-off-by: Kento Yabuuchi * update README Signed-off-by: Kento Yabuuchi * improve format and variable names Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- .../pose_twist_estimator/yabloc.launch.xml | 2 +- .../yabloc/yabloc_image_processing/README.md | 13 +++-- .../launch/image_processing.launch.xml | 3 +- .../src/undistort/undistort_node.cpp | 53 ++++++++++++++----- 4 files changed, 53 insertions(+), 18 deletions(-) diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml index a9b0b93947054..8f0191d6156fe 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/yabloc.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/README.md b/localization/yabloc/yabloc_image_processing/README.md index eb9ae658c39a7..85e1d408a1b4b 100644 --- a/localization/yabloc/yabloc_image_processing/README.md +++ b/localization/yabloc/yabloc_image_processing/README.md @@ -108,10 +108,15 @@ This node performs image resizing and undistortion at the same time. #### Input -| Name | Type | Description | -| ---------------------------- | ----------------------------------- | -------------------- | -| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | raw compressed image | -| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | raw sensor info | +| Name | Type | Description | +| ---------------------------- | ----------------------------------- | ----------------------- | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | raw camera image | +| `input/image_raw/compressed` | `sensor_msgs::msg::CompressedImage` | compressed camera image | + +This node subscribes to both compressed image and raw image topics. +If raw image is subscribed to even once, compressed image will no longer be subscribed to. +This is to avoid redundant decompression within Autoware. #### Output diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index fd55a8163123a..aa6fc10ee6667 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -1,5 +1,5 @@ - + @@ -9,6 +9,7 @@ + diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7412db28e1550..7fc9ad785dbe2 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -50,9 +50,12 @@ class UndistortNode : public rclcpp::Node } auto on_image = std::bind(&UndistortNode::on_image, this, _1); + auto on_compressed_image = std::bind(&UndistortNode::on_compressed_image, this, _1); auto on_info = std::bind(&UndistortNode::on_info, this, _1); - sub_image_ = - create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_image_ = create_subscription("~/input/image_raw", qos, std::move(on_image)); + sub_compressed_image_ = create_subscription( + "~/input/image_raw/compressed", qos, std::move(on_compressed_image)); + sub_info_ = create_subscription("~/input/camera_info", qos, std::move(on_info)); pub_info_ = create_publisher("~/output/resized_info", 10); @@ -63,7 +66,8 @@ class UndistortNode : public rclcpp::Node const int OUTPUT_WIDTH; const std::string OVERRIDE_FRAME_ID; - rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_image_; + rclcpp::Subscription::SharedPtr sub_compressed_image_; rclcpp::Subscription::SharedPtr sub_info_; rclcpp::Publisher::SharedPtr pub_image_; rclcpp::Publisher::SharedPtr pub_info_; @@ -99,14 +103,8 @@ class UndistortNode : public rclcpp::Node scaled_info_->height = new_size.height; } - void on_image(const CompressedImage & msg) + void remap_and_publish(const cv::Mat & image, const std_msgs::msg::Header & header) { - if (!info_.has_value()) return; - if (undistort_map_x.empty()) make_remap_lut(); - - tier4_autoware_utils::StopWatch stop_watch; - cv::Mat image = common::decompress_to_cv_mat(msg); - cv::Mat undistorted_image; cv::remap(image, undistorted_image, undistort_map_x, undistort_map_y, cv::INTER_LINEAR); @@ -120,16 +118,47 @@ class UndistortNode : public rclcpp::Node // Publish Image { cv_bridge::CvImage bridge; - bridge.header.stamp = msg.header.stamp; + bridge.header.stamp = header.stamp; if (OVERRIDE_FRAME_ID != "") bridge.header.frame_id = OVERRIDE_FRAME_ID; else - bridge.header.frame_id = msg.header.frame_id; + bridge.header.frame_id = header.frame_id; bridge.encoding = "bgr8"; bridge.image = undistorted_image; pub_image_->publish(*bridge.toImageMsg()); } + } + void on_image(const Image & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + // To remove redundant decompression, deactivate compressed image subscriber + if (sub_compressed_image_) { + sub_compressed_image_.reset(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); + RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); + } + + void on_compressed_image(const CompressedImage & msg) + { + if (!info_.has_value()) { + return; + } + if (undistort_map_x.empty()) { + make_remap_lut(); + } + + tier4_autoware_utils::StopWatch stop_watch; + remap_and_publish(common::decompress_to_cv_mat(msg), msg.header); RCLCPP_INFO_STREAM(get_logger(), "image undistort: " << stop_watch.toc() << "[ms]"); } From 5fed746365fd647c61eae400fbddc304088b9d4c Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Wed, 4 Oct 2023 16:47:58 +0900 Subject: [PATCH 030/206] chore: fix spell-check errors in perception packages (#5215) fix: cspell error Signed-off-by: Shunsuke Miura --- .../src/lidar_apollo_segmentation_tvm.cpp | 1 + perception/multi_object_tracker/models.md | 2 +- perception/traffic_light_classifier/README.md | 4 +++- 3 files changed, 5 insertions(+), 2 deletions(-) diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index 905d70235e33e..e6c95f202106d 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -22,6 +22,7 @@ #include #include +// cspell: ignore bcnn using model_zoo::perception::lidar_obstacle_detection::baidu_cnn::onnx_bcnn::config; namespace autoware diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index c83038ffa7b59..f74967943be32 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -2,7 +2,7 @@ ## Tracking model - + ### CTRV model [1] diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 7df0c5466695b..2aecf66f8fb7b 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -148,6 +148,8 @@ DATASET_ROOT #### Prerequisites + + **Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html). **Step 2.** Create a conda virtual environment and activate it @@ -295,7 +297,7 @@ python tools/train.py configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py Training logs and weights will be saved in the `work_dirs/mobilenet-v2_8xb32_custom` folder. -### Convert PyTorh model to ONNX model +### Convert PyTorch model to ONNX model #### Install mmdeploy From 36c17b338e390a7f40e1f0ee216c95e9354675f3 Mon Sep 17 00:00:00 2001 From: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> Date: Wed, 4 Oct 2023 20:33:34 +0900 Subject: [PATCH 031/206] refactor(heatmap_visualizer): add JSON Schema and remove default value definition (#4901) * refactor: add JSON Schema and remove default value defined in `declare_parameter()` Signed-off-by: ktro2828 * refactor: add configuration file and update launcher to load this Signed-off-by: ktro2828 * docs: update the docuemnt of core parameters Signed-off-by: ktro2828 * feat: add config into install to share Signed-off-by: ktro2828 --------- Signed-off-by: ktro2828 --- perception/heatmap_visualizer/CMakeLists.txt | 1 + perception/heatmap_visualizer/README.md | 17 ++-- .../config/heatmap_visualizer.param.yaml | 33 ++++++++ .../launch/heatmap_visualizer.launch.xml | 14 +--- .../schema/heatmap_visualizer.schema.json | 81 +++++++++++++++++++ .../src/heatmap_visualizer_node.cpp | 14 ++-- 6 files changed, 133 insertions(+), 27 deletions(-) create mode 100644 perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml create mode 100644 perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json diff --git a/perception/heatmap_visualizer/CMakeLists.txt b/perception/heatmap_visualizer/CMakeLists.txt index ed995491f2af6..75fa02a1cdcf7 100644 --- a/perception/heatmap_visualizer/CMakeLists.txt +++ b/perception/heatmap_visualizer/CMakeLists.txt @@ -16,4 +16,5 @@ rclcpp_components_register_node(heatmap_visualizer_component ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/heatmap_visualizer/README.md b/perception/heatmap_visualizer/README.md index dd8b6f2a7f2b2..747a158b3d487 100644 --- a/perception/heatmap_visualizer/README.md +++ b/perception/heatmap_visualizer/README.md @@ -44,14 +44,15 @@ When publishing, firstly these values are normalized to [0, 1] using maximum and ### Core Parameters -| Name | Type | Default Value | Description | -| ----------------------------- | ------ | ------------- | ------------------------------------------------- | -| `frame_count` | int | `50` | The number of frames to publish heatmap | -| `map_frame` | string | `base_link` | the frame of heatmap to be respected | -| `map_length` | float | `200.0` | the length of map in meter | -| `map_resolution` | float | `0.8` | the resolution of map | -| `use_confidence` | bool | `false` | the flag if use confidence score as heatmap value | -| `rename_car_to_truck_and_bus` | bool | `true` | the flag if rename car to truck or bus | +| Name | Type | Default Value | Description | +| --------------------- | ------------- | ------------------------------------------------------------------------------------- | ----------------------------------------------- | +| `publish_frame_count` | int | `50` | The number of frames to publish heatmap | +| `heatmap_frame_id` | string | `base_link` | The frame ID of heatmap to be respected | +| `heatmap_length` | float | `200.0` | A length of map in meter | +| `heatmap_resolution` | float | `0.8` | A resolution of map | +| `use_confidence` | bool | `false` | A flag if use confidence score as heatmap value | +| `class_names` | array | `["UNKNOWN", "CAR", "TRUCK", "BUS", "TRAILER", "BICYCLE", "MOTORBIKE", "PEDESTRIAN"]` | An array of class names to be published | +| `rename_to_car` | bool | `true` | A flag if rename car like vehicle to car | ## Assumptions / Known limits diff --git a/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml new file mode 100644 index 0000000000000..2da863ea5e37a --- /dev/null +++ b/perception/heatmap_visualizer/config/heatmap_visualizer.param.yaml @@ -0,0 +1,33 @@ +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +/**: + ros__parameters: + publish_frame_count: 50 + heatmap_frame_id: "base_link" + heatmap_length: 200.0 + heatmap_resolution: 0.8 + use_confidence: false + class_names: + [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN", + ] + rename_to_car: false diff --git a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml index e87ae4e63a4d1..7251d776b6110 100644 --- a/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml +++ b/perception/heatmap_visualizer/launch/heatmap_visualizer.launch.xml @@ -2,20 +2,10 @@ - - - - - - + - - - - - - + diff --git a/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json new file mode 100644 index 0000000000000..cb67ae383c35a --- /dev/null +++ b/perception/heatmap_visualizer/schema/heatmap_visualizer.schema.json @@ -0,0 +1,81 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Heatmap Visualizer Node", + "type": "object", + "definitions": { + "heatmap_visualizer": { + "type": "object", + "properties": { + "publish_frame_count": { + "type": "integer", + "description": "The number of frames to be published at once.", + "default": 50, + "minimum": 1 + }, + "heatmap_frame_id": { + "type": "string", + "description": "A frame ID in which visualized heatmap is with respect to.", + "default": "base_link" + }, + "heatmap_length": { + "type": "number", + "description": "A size length of heatmap [m].", + "default": 200.0, + "exclusiveMinimum": 0.0 + }, + "heatmap_resolution": { + "type": "number", + "description": "A cell resolution of heatmap [m].", + "default": 0.8, + "exclusiveMinimum": 0.0 + }, + "use_confidence": { + "type": "boolean", + "description": "Indicates whether to use object existence probability as score. It this parameter is false, 1.0 is set to score.", + "default": false + }, + "class_names": { + "type": "array", + "description": "An array of object class names.", + "default": [ + "UNKNOWN", + "CAR", + "TRUCK", + "BUS", + "TRAILER", + "BICYCLE", + "MOTORBIKE", + "PEDESTRIAN" + ], + "uniqueItems": true + }, + "rename_to_car": { + "type": "boolean", + "description": "Indicates whether to rename car like vehicle into car.", + "default": false + } + }, + "required": [ + "publish_frame_count", + "heatmap_frame_id", + "heatmap_length", + "heatmap_resolution", + "use_confidence", + "class_names", + "rename_to_car" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/heatmap_visualizer" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp index ad57d8638065f..d9337e0c9f61d 100644 --- a/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp +++ b/perception/heatmap_visualizer/src/heatmap_visualizer_node.cpp @@ -27,13 +27,13 @@ namespace heatmap_visualizer HeatmapVisualizerNode::HeatmapVisualizerNode(const rclcpp::NodeOptions & node_options) : Node("heatmap_visualizer", node_options), frame_count_(0) { - total_frame_ = declare_parameter("frame_count", 50); - map_frame_ = declare_parameter("map_frame", "base_link"); - map_length_ = declare_parameter("map_length", 200.0); - map_resolution_ = declare_parameter("map_resolution", 0.8); - use_confidence_ = declare_parameter("use_confidence", false); - class_names_ = declare_parameter("class_names", class_names_); - rename_car_to_truck_and_bus_ = declare_parameter("rename_car_to_truck_and_bus", false); + total_frame_ = static_cast(declare_parameter("publish_frame_count")); + map_frame_ = declare_parameter("heatmap_frame_id"); + map_length_ = static_cast(declare_parameter("heatmap_length")); + map_resolution_ = static_cast(declare_parameter("heatmap_resolution")); + use_confidence_ = declare_parameter("use_confidence"); + class_names_ = declare_parameter>("class_names"); + rename_car_to_truck_and_bus_ = declare_parameter("rename_to_car"); width_ = static_cast(map_length_ / map_resolution_); height_ = static_cast(map_length_ / map_resolution_); From 8770e1e7ff25d951b539b7d48f4e30024aa15cd8 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 4 Oct 2023 21:17:07 +0900 Subject: [PATCH 032/206] feat(intersection): ignore high curvature lane occlusion (#5223) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 2 + .../src/debug.cpp | 8 ++++ .../src/manager.cpp | 4 ++ .../src/scene_intersection.cpp | 5 ++- .../src/scene_intersection.hpp | 2 + .../src/util.cpp | 43 ++++++++++++++++++- .../src/util.hpp | 3 +- .../src/util_type.hpp | 1 + 8 files changed, 64 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 5e74e837bf186..ccd3612203af5 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -57,6 +57,8 @@ absence_traffic_light: creep_velocity: 1.388 # [m/s] maximum_peeking_distance: 6.0 # [m] + attention_lane_crop_curvature_threshold: 0.25 + attention_lane_curvature_calculation_ds: 0.5 enable_rtc: intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index c875611b16003..9ff638cb61876 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -173,6 +173,14 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } + if (debug_data_.occlusion_attention_area) { + appendMarkerArray( + createLaneletPolygonsMarkerArray( + debug_data_.occlusion_attention_area.value(), "occlusion_attention_area", lane_id_, 0.917, + 0.568, 0.596), + &debug_marker_array); + } + if (debug_data_.adjacent_area) { appendMarkerArray( createLaneletPolygonsMarkerArray( diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index d42c9b3ca2aa2..6f47dfbbe31fb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -140,6 +140,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); + ip.occlusion.attention_lane_crop_curvature_threshold = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 89cdeaae56723..10f1ed73bf435 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -989,6 +989,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); debug_data_.attention_area = intersection_lanelets.attention_area(); + debug_data_.occlusion_attention_area = occlusion_attention_area; debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area @@ -1027,7 +1028,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if (!occlusion_attention_divisions_) { occlusion_attention_divisions_ = util::generateDetectionLaneDivisions( occlusion_attention_lanelets, routing_graph_ptr, - planner_data_->occupancy_grid->info.resolution); + planner_data_->occupancy_grid->info.resolution, + planner_param_.occlusion.attention_lane_crop_curvature_threshold, + planner_param_.occlusion.attention_lane_curvature_calculation_ds); } const auto & occlusion_attention_divisions = occlusion_attention_divisions_.value(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 7bd923ed6198c..e75c57ffb3fc7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -119,6 +119,8 @@ class IntersectionModule : public SceneModuleInterface double creep_velocity; double maximum_peeking_distance; } absence_traffic_light; + double attention_lane_crop_curvature_threshold; + double attention_lane_curvature_calculation_ds; } occlusion; }; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index f381925b13208..2f3fcc6f6fde0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -42,6 +43,22 @@ #include #include #include + +namespace tier4_autoware_utils +{ + +template <> +inline geometry_msgs::msg::Point getPoint(const lanelet::ConstPoint3d & p) +{ + geometry_msgs::msg::Point point; + point.x = p.x(); + point.y = p.y(); + point.z = p.z(); + return point; +} + +} // namespace tier4_autoware_utils + namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -809,10 +826,26 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel( return TrafficPrioritizedLevel::NOT_PRIORITIZED; } +double getHighestCurvature(const lanelet::ConstLineString3d & centerline) +{ + std::vector points; + for (auto point = centerline.begin(); point != centerline.end(); point++) { + points.push_back(*point); + } + + SplineInterpolationPoints2d interpolation(points); + const std::vector curvatures = interpolation.getSplineInterpolatedCurvatures(); + std::vector curvatures_positive; + for (const auto & curvature : curvatures) { + curvatures_positive.push_back(std::fabs(curvature)); + } + return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); +} + std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, - [[maybe_unused]] const lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const double resolution) + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; using lanelet::utils::to2D; @@ -824,6 +857,12 @@ std::vector generateDetectionLaneDivisions( if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { continue; } + const auto fine_centerline = + lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); + const double highest_curvature = getHighestCurvature(fine_centerline); + if (highest_curvature > curvature_threshold) { + continue; + } detection_lanelets.push_back(detection_lanelet); } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 6acc1d60443bf..a75545353fb7a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -109,7 +109,8 @@ TrafficPrioritizedLevel getTrafficPrioritizedLevel( std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, - const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution); + const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, + const double curvature_threshold, const double curvature_calculation_ds); std::optional generateInterpolatedPath( const int lane_id, const std::set & associative_lane_ids, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 5c188f4aebebf..aed81d771e480 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -39,6 +39,7 @@ struct DebugData std::optional occlusion_first_stop_wall_pose{std::nullopt}; std::optional pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; + std::optional> occlusion_attention_area{std::nullopt}; std::optional intersection_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; From 98004cedb5059b863f6c964cbacf6812736bd52b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 4 Oct 2023 17:06:57 +0200 Subject: [PATCH 033/206] build(yabloc_common): add missing libgoogle-glog-dev dependency (#5225) * build(yabloc_common): add missing libgoogle-glog-dev dependency Signed-off-by: Esteve Fernandez * style(pre-commit): autofix --------- Signed-off-by: Esteve Fernandez Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/yabloc/yabloc_common/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index f3f46952dec13..d7e94ebefa24b 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -16,6 +16,7 @@ geometry_msgs lanelet2_core lanelet2_extension + libgoogle-glog-dev pcl_conversions rclcpp sensor_msgs From 8649d930353151e4fc656e5f2b8a67f2de35f535 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 5 Oct 2023 09:14:48 +0900 Subject: [PATCH 034/206] refactor(safety_check): support lambda function in target filtering logic (#5210) * feat(safety_check): make it possible to set various conditions Signed-off-by: satoshi-ota * feat(utils): add util func Signed-off-by: satoshi-ota * refactor(start_planner): use common function Signed-off-by: satoshi-ota * refactor(avoidance): use common function Signed-off-by: satoshi-ota * refactor(goal_planner): use common function Signed-off-by: satoshi-ota * refactor(ABLC): use common function Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../path_safety_checker/objects_filtering.hpp | 6 +- .../behavior_path_planner/utils/utils.hpp | 2 + .../goal_planner/goal_planner_module.cpp | 8 ++- .../lane_change/avoidance_by_lane_change.cpp | 8 ++- .../start_planner/start_planner_module.cpp | 8 ++- .../src/utils/avoidance/utils.cpp | 65 ++++++++++--------- .../src/utils/goal_planner/goal_searcher.cpp | 7 +- .../path_safety_checker/objects_filtering.cpp | 26 ++------ .../start_planner/geometric_pull_out.cpp | 8 ++- .../utils/start_planner/shift_pull_out.cpp | 9 ++- .../behavior_path_planner/src/utils/utils.cpp | 13 ++++ 11 files changed, 102 insertions(+), 58 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index e1256fd774027..4d5b9052a4681 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -117,14 +117,16 @@ void filterObjectsByClass( * lanelet. */ std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Separate the objects into two part based on whether the object is within lanelet. * @return Objects pair. first objects are in the lanelet, and second others are out of lanelet. */ std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets); + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition); /** * @brief Get the predicted path from an object. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index c5e5aa3e50191..3e03a09d3adf8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -336,6 +336,8 @@ std::optional getSignedDistanceFromBoundary( // misc +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ce88760212d90..4de8720a4a52d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -21,6 +21,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -1300,12 +1301,17 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return false; } + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); + *(planner_data_->dynamic_object), pull_over_lanes, condition); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 6d06b26cd1e17..ce49501b78d08 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -140,9 +141,14 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets); + *planner_data_->dynamic_object, data.current_lanelets, condition); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 092b91499252c..f19bcf30e7771 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -770,9 +771,14 @@ std::vector StartPlannerModule::searchPullOutStartPoses( std::vector pull_out_start_pose{}; // filter pull out lanes stop objects + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes); + *planner_data_->dynamic_object, status_.pull_out_lanes, condition); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index b2b4ecd2ea42a..86886e4414838 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -564,25 +564,6 @@ double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const return v * std::cos(calcYawDeviation(p_ref, p_target)); } -bool isCentroidWithinLanelets( - const PredictedObject & object, const lanelet::ConstLanelets & target_lanelets) -{ - if (target_lanelets.empty()) { - return false; - } - - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } - - return false; -} - lanelet::ConstLanelets getTargetLanelets( const std::shared_ptr & planner_data, lanelet::ConstLanelets & route_lanelets, const double left_offset, const double right_offset) @@ -1591,14 +1572,26 @@ std::vector getSafetyCheckTargetObjects( std::vector target_objects; - const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) { - std::for_each(objects.begin(), objects.end(), [&](const auto & object) { - if (isCentroidWithinLanelets(object.object, check_lanes)) { - target_objects.push_back(utils::avoidance::transform(object.object, p)); - } + const auto append = [&](const auto & objects) { + std::for_each(objects.objects.begin(), objects.objects.end(), [&](const auto & object) { + target_objects.push_back(utils::avoidance::transform(object, p)); }); }; + const auto to_predicted_objects = [&p](const auto & objects) { + PredictedObjects ret{}; + std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) { + ret.objects.push_back(object.object); + }); + return ret; + }; + + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); + }; + const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1614,11 +1607,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, true); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } @@ -1627,11 +1624,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = getAdjacentLane(planner_data, p, false); if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } @@ -1640,11 +1641,15 @@ std::vector getSafetyCheckTargetObjects( const auto check_lanes = data.current_lanelets; if (p->check_other_object) { - append_target_objects(check_lanes, data.other_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(data.other_objects), check_lanes, condition); + append(targets); } if (p->check_unavoidable_object) { - append_target_objects(check_lanes, unavoidable_objects); + const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( + to_predicted_objects(unavoidable_objects), check_lanes, condition); + append(targets); } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 11173154f6b3a..bd374c12427bc 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -266,13 +266,18 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes); + utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes, condition); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index ff54ce2d2dcc4..e63d35f7bc925 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -128,7 +128,8 @@ void filterObjectsByClass( } std::pair, std::vector> separateObjectIndicesByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { if (target_lanelets.empty()) { return {}; @@ -138,25 +139,9 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - // create object polygon - const auto & obj = objects.objects.at(i); - // create object polygon - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj); bool is_filtered_object = false; for (const auto & llt : target_lanelets) { - // create lanelet polygon - const auto polygon2d = llt.polygon2d().basicPolygon(); - if (polygon2d.empty()) { - // no lanelet polygon - continue; - } - Polygon2d lanelet_polygon; - for (const auto & lanelet_point : polygon2d) { - lanelet_polygon.outer().emplace_back(lanelet_point.x(), lanelet_point.y()); - } - lanelet_polygon.outer().push_back(lanelet_polygon.outer().front()); - // check the object does not intersect the lanelet - if (!boost::geometry::disjoint(lanelet_polygon, obj_polygon)) { + if (condition(objects.objects.at(i), llt)) { target_indices.push_back(i); is_filtered_object = true; break; @@ -172,13 +157,14 @@ std::pair, std::vector> separateObjectIndicesByLanel } std::pair separateObjectsByLanelets( - const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets) + const PredictedObjects & objects, const lanelet::ConstLanelets & target_lanelets, + const std::function & condition) { PredictedObjects target_objects; PredictedObjects other_objects; const auto [target_indices, other_indices] = - separateObjectIndicesByLanelets(objects, target_lanelets); + separateObjectIndicesByLanelets(objects, target_lanelets, condition); target_objects.objects.reserve(target_indices.size()); other_objects.objects.reserve(other_indices.size()); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index bc071e2a2a288..f4bfb4d681440 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -18,6 +18,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -62,10 +63,15 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), 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::separateObjectsByLanelets(stop_objects, pull_out_lanes, condition); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index b82157cc17e41..02c6d41bdbf08 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -64,8 +65,14 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P } // extract stop objects in pull out lane for collision check + const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); + }; const auto [pull_out_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(*dynamic_objects, pull_out_lanes); + utils::path_safety_checker::separateObjectsByLanelets( + *dynamic_objects, pull_out_lanes, condition); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_.th_moving_object_velocity); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 7305405b50410..03063b5a9e2fe 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2675,6 +2675,19 @@ double getArcLengthToTargetLanelet( std::min(arc_front.length - arc_pose.length, arc_back.length - arc_pose.length), 0.0); } +Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) +{ + Polygon2d polygon; + for (const auto & p : lanelet.polygon2d().basicPolygon()) { + polygon.outer().emplace_back(p.x(), p.y()); + } + polygon.outer().push_back(polygon.outer().front()); + + return tier4_autoware_utils::isClockwise(polygon) + ? polygon + : tier4_autoware_utils::inverseClockwise(polygon); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type) From c763eda68ede1ccc5b50290cb61f5775061aa212 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 5 Oct 2023 11:32:43 +0900 Subject: [PATCH 035/206] fix(detected_object_validation): change the points_num of the validator to be set class by class (#5177) * fix: add param for each object class Signed-off-by: badai-nguyen * fix: add missing classes param Signed-off-by: badai-nguyen * fix: launch file Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...mera_lidar_fusion_based_detection.launch.xml | 1 + ...idar_radar_fusion_based_detection.launch.xml | 1 + .../detection/lidar_based_detection.launch.xml | 1 + .../launch/perception.launch.xml | 1 + ...stacle_pointcloud_based_validator.param.yaml | 13 +++++++++++++ .../obstacle_pointcloud_based_validator.hpp | 8 ++++---- ...stacle_pointcloud_based_validator.launch.xml | 2 ++ .../src/obstacle_pointcloud_based_validator.cpp | 17 ++++++++++++----- 8 files changed, 35 insertions(+), 9 deletions(-) create mode 100644 perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 178e193dc2d99..51359d8d8f0e2 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -314,6 +314,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 38d141241c215..45cfb91bf25c1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -348,6 +348,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 3b6b9ba652a24..c844db39f4e9d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -115,6 +115,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index b0219376e9625..3d4ce1035e72c 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -17,6 +17,7 @@ + diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml new file mode 100644 index 0000000000000..745a0fd6591a9 --- /dev/null +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + min_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + max_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + min_points_and_distance_ratio: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 270e5c7bdb7ff..5819302664907 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -32,14 +32,14 @@ #include #include - +#include namespace obstacle_pointcloud_based_validator { struct PointsNumThresholdParam { - size_t min_points_num; - size_t max_points_num; - float min_points_and_distance_ratio; + std::vector min_points_num; + std::vector max_points_num; + std::vector min_points_and_distance_ratio; }; class ObstaclePointCloudBasedValidator : public rclcpp::Node { diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 8196e95f37957..799b605658345 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -3,11 +3,13 @@ + + diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 00e53e9de9a9c..47640c9332e4d 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -92,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - points_num_threshold_param_.min_points_num = declare_parameter("min_points_num", 10); - points_num_threshold_param_.max_points_num = declare_parameter("max_points_num", 10); + points_num_threshold_param_.min_points_num = + declare_parameter>("min_points_num"); + points_num_threshold_param_.max_points_num = + declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = - declare_parameter("min_points_and_distance_ratio", 800.0); + declare_parameter>("min_points_and_distance_ratio"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -134,6 +136,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; const auto & object = input_objects->objects.at(i); const auto & transformed_object_position = transformed_object.kinematics.pose_with_covariance.pose.position; @@ -155,13 +158,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); // Filter object that have few pointcloud in them. + // TODO(badai-nguyen) add 3d validator option const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); const auto object_distance = std::hypot(transformed_object_position.x, transformed_object_position.y); size_t min_pointcloud_num = std::clamp( static_cast( - points_num_threshold_param_.min_points_and_distance_ratio / object_distance + 0.5f), - points_num_threshold_param_.min_points_num, points_num_threshold_param_.max_points_num); + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); if (num) { (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) : removed_objects.objects.push_back(object); From 02fda2f44217bdd57690302460f16ff3b9c4dcbe Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 5 Oct 2023 13:16:32 +0900 Subject: [PATCH 036/206] refactor(safety_check): use common function in safety check library (#5217) refactor(avoidance): use common function Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 10 ++- .../utils/avoidance/avoidance_module_data.hpp | 6 +- .../utils/avoidance/utils.hpp | 8 --- .../avoidance/avoidance_module.cpp | 14 ++-- .../src/scene_module/avoidance/manager.cpp | 22 ++++-- .../src/utils/avoidance/utils.cpp | 69 ++----------------- 6 files changed, 43 insertions(+), 86 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 589233cf5a441..df2dd806fe2c7 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -139,12 +139,16 @@ check_other_object: true # [-] # collision check parameters check_all_predicted_path: false # [-] - time_resolution: 0.5 # [s] - time_horizon_for_front_object: 10.0 # [s] - time_horizon_for_rear_object: 10.0 # [s] safety_check_backward_distance: 100.0 # [m] hysteresis_factor_expand_rate: 2.0 # [-] hysteresis_factor_safe_count: 10 # [-] + # predicted path parameters + min_velocity: 1.38 # [m/s] + max_velocity: 50.0 # [m/s] + time_resolution: 0.5 # [s] + time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_rear_object: 10.0 # [s] + delay_until_departure: 0.0 # [s] # rss parameters expected_front_deceleration: -1.0 # [m/ss] expected_rear_deceleration: -1.0 # [m/ss] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 634ac69bae12c..6173ecb15a5c2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -183,9 +183,6 @@ struct AvoidanceParameters // parameters for collision check. bool check_all_predicted_path{false}; - double time_horizon_for_front_object{0.0}; - double time_horizon_for_rear_object{0.0}; - double safety_check_time_resolution{0.0}; // find adjacent lane vehicles double safety_check_backward_distance{0.0}; @@ -295,6 +292,9 @@ struct AvoidanceParameters // parameters depend on object class std::unordered_map object_parameters; + // ego predicted path params. + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + // rss parameters utils::path_safety_checker::RSSparams rss_params{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 44d5d6f6b4e56..c0bd621cc86b4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -87,11 +87,6 @@ std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width); -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const bool limit_to_max_velocity, - const std::shared_ptr & parameters); - double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); bool isCentroidWithinLanelets( @@ -162,9 +157,6 @@ AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine AvoidLineArray combineRawShiftLinesWithUniqueCheck( const AvoidLineArray & base_lines, const AvoidLineArray & added_lines); -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters); - std::vector getSafetyCheckTargetObjects( const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 36908bd87de23..a8dd0b794245e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -1839,10 +1839,16 @@ bool AvoidanceModule::isSafePath( } const bool limit_to_max_velocity = false; - const auto ego_predicted_path_for_front_object = utils::avoidance::convertToPredictedPath( - shifted_path.path, planner_data_, true, limit_to_max_velocity, parameters_); - const auto ego_predicted_path_for_rear_object = utils::avoidance::convertToPredictedPath( - shifted_path.path, planner_data_, false, limit_to_max_velocity, parameters_); + const auto ego_predicted_path_params = + std::make_shared( + parameters_->ego_predicted_path_params); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(shifted_path.path.points); + const auto ego_predicted_path_for_front_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + true, limit_to_max_velocity); + const auto ego_predicted_path_for_rear_object = utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, shifted_path.path.points, getEgoPose(), getEgoSpeed(), ego_seg_idx, + false, limit_to_max_velocity); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 912f867fd5701..8c65323e5a123 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -142,11 +142,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.check_other_object = getOrDeclareParameter(*node, ns + "check_other_object"); p.check_all_predicted_path = getOrDeclareParameter(*node, ns + "check_all_predicted_path"); - p.time_horizon_for_front_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_front_object"); - p.time_horizon_for_rear_object = - getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); - p.safety_check_time_resolution = getOrDeclareParameter(*node, ns + "time_resolution"); p.safety_check_backward_distance = getOrDeclareParameter(*node, ns + "safety_check_backward_distance"); p.hysteresis_factor_expand_rate = @@ -155,6 +150,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "hysteresis_factor_safe_count"); } + // safety check predicted path params + { + std::string ns = "avoidance.safety_check."; + p.ego_predicted_path_params.min_velocity = + getOrDeclareParameter(*node, ns + "min_velocity"); + p.ego_predicted_path_params.max_velocity = + getOrDeclareParameter(*node, ns + "max_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(*node, "avoidance.constraints.longitudinal.max_acceleration"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(*node, ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(*node, ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(*node, ns + "delay_until_departure"); + } + // safety check rss params { std::string ns = "avoidance.safety_check."; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 86886e4414838..d7da362402197 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1457,68 +1457,6 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( return combined; } -std::vector convertToPredictedPath( - const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const bool limit_to_max_velocity, - const std::shared_ptr & parameters) -{ - const auto & vehicle_pose = planner_data->self_odometry->pose.pose; - const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); - - auto ego_predicted_path_params = - std::make_shared(); - - ego_predicted_path_params->min_velocity = parameters->min_slow_down_speed; - ego_predicted_path_params->acceleration = parameters->max_acceleration; - ego_predicted_path_params->max_velocity = std::numeric_limits::infinity(); - ego_predicted_path_params->time_horizon_for_front_object = - parameters->time_horizon_for_front_object; - ego_predicted_path_params->time_horizon_for_rear_object = - parameters->time_horizon_for_rear_object; - ego_predicted_path_params->time_resolution = parameters->safety_check_time_resolution; - ego_predicted_path_params->delay_until_departure = 0.0; - - return behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params, path.points, vehicle_pose, initial_velocity, ego_seg_idx, - is_object_front, limit_to_max_velocity); -} - -ExtendedPredictedObject transform( - const PredictedObject & object, const std::shared_ptr & parameters) -{ - ExtendedPredictedObject extended_object; - extended_object.uuid = object.object_id; - extended_object.initial_pose = object.kinematics.initial_pose_with_covariance; - extended_object.initial_twist = object.kinematics.initial_twist_with_covariance; - extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance; - extended_object.shape = object.shape; - - const auto & obj_velocity_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); - const auto & time_horizon = - std::max(parameters->time_horizon_for_front_object, parameters->time_horizon_for_rear_object); - const auto & time_resolution = parameters->safety_check_time_resolution; - - extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); - for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { - const auto & path = object.kinematics.predicted_paths.at(i); - extended_object.predicted_paths.at(i).confidence = path.confidence; - - // create path - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t); - if (obj_pose) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape); - extended_object.predicted_paths.at(i).path.emplace_back( - t, *obj_pose, obj_velocity_norm, obj_polygon); - } - } - } - - return extended_object; -} - lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & planner_data, const std::shared_ptr & parameters, const bool is_right_shift) @@ -1572,9 +1510,14 @@ std::vector getSafetyCheckTargetObjects( std::vector target_objects; + const auto time_horizon = std::max( + parameters->ego_predicted_path_params.time_horizon_for_front_object, + parameters->ego_predicted_path_params.time_horizon_for_rear_object); + const auto append = [&](const auto & objects) { std::for_each(objects.objects.begin(), objects.objects.end(), [&](const auto & object) { - target_objects.push_back(utils::avoidance::transform(object, p)); + target_objects.push_back(utils::path_safety_checker::transform( + object, time_horizon, parameters->ego_predicted_path_params.time_resolution)); }); }; From dd275bf94e78e5c18d43a2228759812c267d2184 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 5 Oct 2023 14:43:08 +0900 Subject: [PATCH 037/206] fix(lane_change): current lane obj treated as target lane obj (#5214) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d8fa9cb5630e9..c75970e6fcc2d 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -683,6 +683,16 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); std::vector> target_backward_polygons; for (const auto & target_backward_lane : target_backward_lanes) { + // Check to see is target_backward_lane is in current_lanes + // Without this check, current lane object might be treated as target lane object + const auto is_current_lane = [&](const lanelet::ConstLanelet & current_lane) { + return current_lane.id() == target_backward_lane.id(); + }; + + if (std::any_of(current_lanes.begin(), current_lanes.end(), is_current_lane)) { + continue; + } + lanelet::ConstLanelets lanelet{target_backward_lane}; auto lane_polygon = utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); From 877b04b133175fc39444a9400cd83ae8f22de5b4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 5 Oct 2023 15:51:15 +0900 Subject: [PATCH 038/206] fix(blind_spot): fix runtime crash (#5227) Signed-off-by: Mamoru Sobue --- planning/behavior_velocity_blind_spot_module/src/debug.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index f95ed73c2a184..7da494bfd5231 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include #include From 8e971bcc8ef480e2791e436d7eb979f82d324e54 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Thu, 5 Oct 2023 17:22:20 +0900 Subject: [PATCH 039/206] fix(behavior_velocity_crosswalk_module): stop at stop line associated with crosswalk (#5231) don't consider margin when stop line is found Signed-off-by: kyoichi-sugahara --- .../behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 62acd191d8697..fe96985743b94 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -299,8 +299,7 @@ std::optional> CrosswalkModule::get const auto p_stop_lines = getLinestringIntersects( ego_path, lanelet::utils::to2D(stop_line).basicLineString(), ego_pos, 2); if (!p_stop_lines.empty()) { - return std::make_pair( - p_stop_lines.front(), -planner_param_.stop_distance_from_object - base_link2front); + return std::make_pair(p_stop_lines.front(), -base_link2front); } } From b0310e769c9184e70a3b2eecd616cd1eb066fd18 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 5 Oct 2023 18:49:30 +0900 Subject: [PATCH 040/206] refactor(safety_check): define filtering function in safety check library (#5228) Signed-off-by: satoshi-ota --- .../path_safety_checker/objects_filtering.hpp | 18 ++++++++++++++ .../goal_planner/goal_planner_module.cpp | 8 ++----- .../lane_change/avoidance_by_lane_change.cpp | 8 ++----- .../start_planner/start_planner_module.cpp | 8 ++----- .../src/utils/avoidance/utils.cpp | 24 +++++++++---------- .../src/utils/goal_planner/goal_searcher.cpp | 8 ++----- .../path_safety_checker/objects_filtering.cpp | 14 +++++++++++ .../start_planner/geometric_pull_out.cpp | 8 ++----- .../utils/start_planner/shift_pull_out.cpp | 7 +----- 9 files changed, 55 insertions(+), 48 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 4d5b9052a4681..15b15bac40b51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -38,6 +38,24 @@ using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +/** + * @brief Filters objects based on object centroid position. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + +/** + * @brief Filters objects based on object polygon overlapping with lanelet. + * + * @param objects The predicted objects to filter. + * @param lanelet + * @return result. + */ +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet); + /** * @brief Filters objects based on various criteria. * diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 4de8720a4a52d..9c2544cf60cec 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1301,17 +1301,13 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return false; } - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length); const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes, condition); + *(planner_data_->dynamic_object), pull_over_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index ce49501b78d08..1f305e3988828 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -141,14 +141,10 @@ void AvoidanceByLaneChange::fillAvoidanceTargetObjects( { const auto p = std::dynamic_pointer_cast(avoidance_parameters_); - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [object_within_target_lane, object_outside_target_lane] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, data.current_lanelets, condition); + *planner_data_->dynamic_object, data.current_lanelets, + utils::path_safety_checker::isPolygonOverlapLanelet); // Assume that the maximum allocation for data.other object is the sum of // objects_within_target_lane and object_outside_target_lane. The maximum allocation for diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index f19bcf30e7771..c828a747856d8 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -771,14 +771,10 @@ std::vector StartPlannerModule::searchPullOutStartPoses( std::vector pull_out_start_pose{}; // filter pull out lanes stop objects - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *planner_data_->dynamic_object, status_.pull_out_lanes, condition); + *planner_data_->dynamic_object, status_.pull_out_lanes, + utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_->th_moving_object_velocity); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index d7da362402197..0e7eb87feed9b 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1529,12 +1529,6 @@ std::vector getSafetyCheckTargetObjects( return ret; }; - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; - lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); - }; - const auto unavoidable_objects = [&data]() { ObjectDataArray ret; std::for_each(data.target_objects.begin(), data.target_objects.end(), [&](const auto & object) { @@ -1551,13 +1545,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } @@ -1568,13 +1564,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } @@ -1585,13 +1583,15 @@ std::vector getSafetyCheckTargetObjects( if (p->check_other_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(data.other_objects), check_lanes, condition); + to_predicted_objects(data.other_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } if (p->check_unavoidable_object) { const auto [targets, others] = utils::path_safety_checker::separateObjectsByLanelets( - to_predicted_objects(unavoidable_objects), check_lanes, condition); + to_predicted_objects(unavoidable_objects), check_lanes, + utils::path_safety_checker::isCentroidWithinLanelet); append(targets); } } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index bd374c12427bc..12044980ebd81 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -266,18 +266,14 @@ void GoalSearcher::countObjectsToAvoid( void GoalSearcher::update(GoalCandidates & goal_candidates) const { - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); const auto [pull_over_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_over_lanes, condition); + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); if (parameters_.prioritize_goals_before_objects) { countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index e63d35f7bc925..dfc9f76b25e33 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -24,6 +24,20 @@ namespace behavior_path_planner::utils::path_safety_checker { +bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); + return boost::geometry::within(object_centroid, lanelet.polygon2d().basicPolygon()); +} + +bool isPolygonOverlapLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); + const auto lanelet_polygon = utils::toPolygon2d(lanelet); + return !boost::geometry::disjoint(lanelet_polygon, object_polygon); +} + PredictedObjects filterObjects( const std::shared_ptr & objects, const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index f4bfb4d681440..3cf42dbb5da26 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -63,15 +63,11 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con // collision check with stop objects in pull out lanes const auto arc_path = planner_.getArcPath(); - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto & stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *(planner_data_->dynamic_object), parameters_.th_moving_object_velocity); const auto [pull_out_lane_stop_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets(stop_objects, pull_out_lanes, condition); + utils::path_safety_checker::separateObjectsByLanelets( + stop_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); if (utils::checkCollisionBetweenPathFootprintsAndObjects( vehicle_footprint_, arc_path, pull_out_lane_stop_objects, diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 02c6d41bdbf08..a430e18c330e1 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -65,14 +65,9 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P } // extract stop objects in pull out lane for collision check - const auto condition = [](const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object); - const auto lanelet_polygon = utils::toPolygon2d(lanelet); - return !boost::geometry::disjoint(lanelet_polygon, object_polygon); - }; const auto [pull_out_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( - *dynamic_objects, pull_out_lanes, condition); + *dynamic_objects, pull_out_lanes, utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_out_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_out_lane_objects, parameters_.th_moving_object_velocity); From e12817df60b2579b1be513950d463b6652c4e717 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Fri, 6 Oct 2023 09:03:28 +0900 Subject: [PATCH 041/206] fix(tier4_perception_launch): fix faraway detection to reduce calculation cost (#5233) * fix(tier4_perception_launch): fix node order in radar_based_detection.launch Signed-off-by: scepter914 * fix comment out unused node Signed-off-by: scepter914 --------- Signed-off-by: scepter914 --- .../detection/radar_based_detection.launch.xml | 16 ++++++++-------- .../tracking/tracking.launch.xml | 2 ++ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 52c8aedff8ef9..5b5646a061ac7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -20,14 +20,8 @@ - - - - - - - + @@ -40,8 +34,14 @@ + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 1ba1e8de9d42e..77de1e5995ee0 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -15,6 +15,7 @@ + From 6dfb197f95771421a7fc9acbdafe2028856ce789 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Fri, 6 Oct 2023 13:53:46 +0900 Subject: [PATCH 042/206] feat(obstacle_cruise_planner): obstacle type dependent slow down for obstacle cruise planner (#5208) * Add slow down parameters dependant on obstacle type Signed-off-by: Daniel Sanchez * divide obstacle parameters based on obstacle type Signed-off-by: Daniel Sanchez * preserve obstacle type for velocity calculation Signed-off-by: Daniel Sanchez * added pre-commit changes Signed-off-by: Daniel Sanchez * change sample config params Signed-off-by: Daniel Sanchez * Update Readme Signed-off-by: Daniel Sanchez * Update README Signed-off-by: Daniel Sanchez * small refactor for cleaner code Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Signed-off-by: Daniel Sanchez --- planning/obstacle_cruise_planner/README.md | 21 +++-- .../config/obstacle_cruise_planner.param.yaml | 17 +++- .../common_structs.hpp | 8 +- .../planner_interface.hpp | 80 +++++++++++++++---- planning/obstacle_cruise_planner/src/node.cpp | 6 +- .../src/planner_interface.cpp | 7 +- 6 files changed, 101 insertions(+), 38 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 314f9dcc29393..260f302791079 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -230,14 +230,19 @@ $$ ### Slow down planning -| Parameter | Type | Description | -| ---------------------------- | ------ | ------------------------------------------------------------------- | -| `slow_down.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m] | -| `slow_down.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m] | -| `slow_down.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m] | - -The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. +| Parameter | Type | Description | +| ----------------------------------------------- | -------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `slow_down.labels` | vector(string) | A vector of labels for customizing obstacle-label-based slow down behavior. Each label represents an obstacle type that will be treated differently when applying slow down. The possible labels are ("default" (Mandatory), "unknown","car","truck","bus","trailer","motorcycle","bicycle" or "pedestrian") | +| `slow_down.default.min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `slow_down.default.max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: This default value will be used when the detected obstacle label does not match any of the slow_down.labels | +| `(optional) slow_down."label".min_lat_velocity` | double | minimum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_velocity` | double | maximum velocity to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".min_lat_margin` | double | minimum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | +| `(optional) slow_down."label".max_lat_margin` | double | maximum lateral margin to linearly calculate slow down velocity [m]. Note: only for obstacles specified in `slow_down.labels` | + +The role of the slow down planning is inserting slow down velocity in the trajectory where the trajectory points are close to the obstacles. The parameters can be customized depending on the obstacle type (see `slow_down.labels`), making it possible to adjust the slow down behavior depending if the obstacle is a pedestrian, bicycle, car, etc. The closest point on the obstacle to the ego's trajectory is calculated. Then, the slow down velocity is calculated by linear interpolation with the distance between the point and trajectory as follows. diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 73bc4a83249a4..123d0dda93b7a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -179,10 +179,19 @@ slow_down: # parameters to calculate slow down velocity by linear interpolation - min_lat_margin: 0.2 - max_lat_margin: 1.0 - min_ego_velocity: 2.0 - max_ego_velocity: 8.0 + labels: + - "default" + - "pedestrian" + default: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + pedestrian: + min_lat_margin: 0.5 + max_lat_margin: 1.5 + min_ego_velocity: 1.0 + max_ego_velocity: 2.0 time_margin_on_target_velocity: 1.5 # [s] diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index e958074971f2a..c6cb73a680e5a 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -137,19 +137,21 @@ struct SlowDownObstacle : public TargetObstacleInterface { SlowDownObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, - const double arg_lat_velocity, const double arg_precise_lat_dist, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const double arg_lon_velocity, const double arg_lat_velocity, const double arg_precise_lat_dist, const geometry_msgs::msg::Point & arg_front_collision_point, const geometry_msgs::msg::Point & arg_back_collision_point) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), precise_lat_dist(arg_precise_lat_dist), front_collision_point(arg_front_collision_point), - back_collision_point(arg_back_collision_point) + back_collision_point(arg_back_collision_point), + classification(object_classification) { } double precise_lat_dist; // for efficient calculation geometry_msgs::msg::Point front_collision_point; geometry_msgs::msg::Point back_collision_point; + ObjectClassification classification; }; struct LongitudinalInfo diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 9a5a6521a6494..d8c31fb35df9b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -221,12 +222,42 @@ class PlannerInterface struct SlowDownParam { + std::vector obstacle_labels{"default"}; + std::unordered_map types_map; + struct ObstacleSpecificParams + { + double max_lat_margin; + double min_lat_margin; + double max_ego_velocity; + double min_ego_velocity; + }; explicit SlowDownParam(rclcpp::Node & node) { - max_lat_margin = node.declare_parameter("slow_down.max_lat_margin"); - min_lat_margin = node.declare_parameter("slow_down.min_lat_margin"); - max_ego_velocity = node.declare_parameter("slow_down.max_ego_velocity"); - min_ego_velocity = node.declare_parameter("slow_down.min_ego_velocity"); + types_map = {{ObjectClassification::UNKNOWN, "unknown"}, + {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, + {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, + {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, + {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + obstacle_labels = + node.declare_parameter>("slow_down.labels", obstacle_labels); + // obstacle label dependant parameters + for (const auto & label : obstacle_labels) { + ObstacleSpecificParams params; + params.max_lat_margin = + node.declare_parameter("slow_down." + label + ".max_lat_margin"); + params.min_lat_margin = + node.declare_parameter("slow_down." + label + ".min_lat_margin"); + params.max_ego_velocity = + node.declare_parameter("slow_down." + label + ".max_ego_velocity"); + params.min_ego_velocity = + node.declare_parameter("slow_down." + label + ".min_ego_velocity"); + obstacle_to_param_struct_map.emplace(std::make_pair(label, params)); + } + + // common parameters time_margin_on_target_velocity = node.declare_parameter("slow_down.time_margin_on_target_velocity"); lpf_gain_slow_down_vel = node.declare_parameter("slow_down.lpf_gain_slow_down_vel"); @@ -235,16 +266,35 @@ class PlannerInterface node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); } + ObstacleSpecificParams getObstacleParamByLabel(const ObjectClassification & label_id) const + { + const std::string label = types_map.at(label_id.label); + if (obstacle_to_param_struct_map.count(label) > 0) { + return obstacle_to_param_struct_map.at(label); + } + return obstacle_to_param_struct_map.at("default"); + } + void onParam(const std::vector & parameters) { - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_lat_margin", max_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_lat_margin", min_lat_margin); - tier4_autoware_utils::updateParam( - parameters, "slow_down.max_ego_velocity", max_ego_velocity); - tier4_autoware_utils::updateParam( - parameters, "slow_down.min_ego_velocity", min_ego_velocity); + // obstacle type dependant parameters + for (const auto & label : obstacle_labels) { + auto & param_by_obstacle_label = obstacle_to_param_struct_map[label]; + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + tier4_autoware_utils::updateParam( + parameters, "slow_down." + label + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } + + // common parameters tier4_autoware_utils::updateParam( parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); tier4_autoware_utils::updateParam( @@ -255,10 +305,8 @@ class PlannerInterface parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); } - double max_lat_margin; - double min_lat_margin; - double max_ego_velocity; - double min_ego_velocity; + std::unordered_map obstacle_to_param_struct_map; + double time_margin_on_target_velocity; double lpf_gain_slow_down_vel; double lpf_gain_lat_dist; diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 7de6bb2c966f6..d50054da46237 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1175,9 +1175,9 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.pose, - tangent_vel, normal_vel, precise_lat_dist, - front_collision_point, back_collision_point}; + return SlowDownObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, tangent_vel, normal_vel, + precise_lat_dist, front_collision_point, back_collision_point}; } void ObstacleCruisePlannerNode::checkConsistency( diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 287df10dcebb5..76469364cfb39 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -665,7 +665,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( double PlannerInterface::calculateSlowDownVelocity( const SlowDownObstacle & obstacle, const std::optional & prev_output) const { - const auto & p = slow_down_param_; + const auto & p = slow_down_param_.getObstacleParamByLabel(obstacle.classification); const double stable_precise_lat_dist = [&]() { if (prev_output) { @@ -691,7 +691,6 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( const SlowDownObstacle & obstacle, const std::optional & prev_output, const double dist_to_ego) const { - const auto & p = slow_down_param_; const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); @@ -728,8 +727,8 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints( // calculate distance during deceleration, slow down preparation, and slow down const double min_slow_down_prepare_dist = 3.0; - const double slow_down_prepare_dist = - std::max(min_slow_down_prepare_dist, slow_down_vel * p.time_margin_on_target_velocity); + const double slow_down_prepare_dist = std::max( + min_slow_down_prepare_dist, slow_down_vel * slow_down_param_.time_margin_on_target_velocity); const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision - abs_ego_offset - dist_to_ego - slow_down_prepare_dist; const double slow_down_dist = From 950805fda122ac31f9784e841ac5106b201bc1ca Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 6 Oct 2023 13:54:15 +0900 Subject: [PATCH 043/206] feat(intersection)!: disable the exception behavior in the private areas (#5229) * feat: make configurable to disable the exception treat of stuck obstacle in the private areas feat: change behavior in the private areas Signed-off-by: Yuki Takagi * delete the coment outed lines Signed-off-by: Yuki Takagi * change "enabled" to "enable" Signed-off-by: Yuki Takagi * fix setting Signed-off-by: Yuki Takagi * delete unused variables Signed-off-by: Yuki Takagi --------- Signed-off-by: Yuki Takagi --- .../config/intersection.param.yaml | 1 + .../src/manager.cpp | 2 ++ .../src/scene_intersection.cpp | 11 ++--------- .../src/scene_intersection.hpp | 2 +- 4 files changed, 6 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index ccd3612203af5..e210d2fb21b88 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -19,6 +19,7 @@ # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area + enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. collision_detection: state_transit_margin_time: 1.0 diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 6f47dfbbe31fb..3c96a171516bd 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -77,6 +77,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) */ ip.stuck_vehicle.timeout_private_area = getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); + ip.stuck_vehicle.enable_private_area_stuck_disregard = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 10f1ed73bf435..b30a57f9e8dad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -96,10 +96,6 @@ IntersectionModule::IntersectionModule( occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); occlusion_stop_state_machine_.setState(StateMachine::State::GO); } - { - stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); - stuck_private_area_timeout_.setState(StateMachine::State::STOP); - } { temporal_stop_before_attention_state_machine_.setMarginTime( planner_param_.occlusion.before_creep_stop_time); @@ -918,16 +914,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); if (stuck_detected && stuck_stop_line_idx_opt) { auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - const bool stopped_at_stuck_line = stoppedForDuration( - stuck_stop_line_idx, planner_param_.stuck_vehicle.timeout_private_area, - stuck_private_area_timeout_); - const bool timeout = (is_private_area_ && stopped_at_stuck_line); - if (!timeout) { + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { if ( default_stop_line_idx_opt && fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { stuck_stop_line_idx = default_stop_line_idx_opt.value(); } + } else { return IntersectionModule::StuckStop{ closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index e75c57ffb3fc7..b5cf3ec04cdc3 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -70,6 +70,7 @@ class IntersectionModule : public SceneModuleInterface bool enable_front_car_decel_prediction; //! flag for using above feature */ double timeout_private_area; + bool enable_private_area_stuck_disregard; } stuck_vehicle; struct CollisionDetection { @@ -255,7 +256,6 @@ class IntersectionModule : public SceneModuleInterface // for stuck vehicle detection const bool is_private_area_; - StateMachine stuck_private_area_timeout_; // for RTC const UUID occlusion_uuid_; From 057d3d4259f69e0c83e7897a954ddac3f4818fcc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 6 Oct 2023 20:16:42 +0900 Subject: [PATCH 044/206] feat(lane_change): separate execution and cancel safety check param (#5246) Signed-off-by: Zulfaqar Azmi --- .../config/lane_change/lane_change.param.yaml | 26 ++++--- .../lane_change/lane_change_module_data.hpp | 1 + .../src/scene_module/lane_change/manager.cpp | 70 +++++++++++++------ 3 files changed, 66 insertions(+), 31 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index fda7390abdb9e..e7f3b51bd2d26 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -28,15 +28,23 @@ # safety check safety_check: - expected_front_deceleration: -1.0 - expected_rear_deceleration: -1.0 - expected_front_deceleration_for_abort: -1.0 - expected_rear_deceleration_for_abort: -2.0 - rear_vehicle_reaction_time: 2.0 - rear_vehicle_safety_time_margin: 1.0 - lateral_distance_max_threshold: 2.0 - longitudinal_distance_min_threshold: 3.0 - longitudinal_velocity_delta_time: 0.8 + allow_loose_check_for_cancel: true + execution: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 + cancel: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -2.0 + rear_vehicle_reaction_time: 1.5 + rear_vehicle_safety_time_margin: 0.8 + lateral_distance_max_threshold: 1.0 + longitudinal_distance_min_threshold: 2.5 + longitudinal_velocity_delta_time: 0.6 # lane expansion for object filtering lane_expansion: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 0ba35635a9143..24a507d1f8695 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -82,6 +82,7 @@ struct LaneChangeParameters utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check + bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 9c45f79d00263..e33a6c4b05ee3 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -92,35 +92,41 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.stop_time_threshold = getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + // safety check + p.allow_loose_check_for_cancel = + getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); - p.rss_params.front_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_front_deceleration")); - p.rss_params.rear_vehicle_deceleration = - getOrDeclareParameter(*node, parameter("safety_check.expected_rear_deceleration")); - p.rss_params.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.execution.longitudinal_velocity_delta_time")); + p.rss_params.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_front_deceleration")); + p.rss_params.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.execution.expected_rear_deceleration")); + p.rss_params.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_reaction_time")); + p.rss_params.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.execution.rear_vehicle_safety_time_margin")); + p.rss_params.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.execution.lateral_distance_max_threshold")); p.rss_params_for_abort.longitudinal_distance_min_threshold = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_distance_min_threshold")); + *node, parameter("safety_check.cancel.longitudinal_distance_min_threshold")); p.rss_params_for_abort.longitudinal_velocity_delta_time = getOrDeclareParameter( - *node, parameter("safety_check.longitudinal_velocity_delta_time")); + *node, parameter("safety_check.cancel.longitudinal_velocity_delta_time")); p.rss_params_for_abort.front_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_front_deceleration_for_abort")); + *node, parameter("safety_check.cancel.expected_front_deceleration")); p.rss_params_for_abort.rear_vehicle_deceleration = getOrDeclareParameter( - *node, parameter("safety_check.expected_rear_deceleration_for_abort")); - p.rss_params_for_abort.rear_vehicle_reaction_time = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_reaction_time")); - p.rss_params_for_abort.rear_vehicle_safety_time_margin = - getOrDeclareParameter(*node, parameter("safety_check.rear_vehicle_safety_time_margin")); - p.rss_params_for_abort.lateral_distance_max_threshold = - getOrDeclareParameter(*node, parameter("safety_check.lateral_distance_max_threshold")); + *node, parameter("safety_check.cancel.expected_rear_deceleration")); + p.rss_params_for_abort.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_reaction_time")); + p.rss_params_for_abort.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.cancel.rear_vehicle_safety_time_margin")); + p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); // target object { @@ -165,6 +171,26 @@ LaneChangeModuleManager::LaneChangeModuleManager( exit(EXIT_FAILURE); } + // validation of safety check parameters + // if loosely check is not allowed, lane change module will keep on chattering and canceling, and + // false positive situation might occur + if (!p.allow_loose_check_for_cancel) { + if ( + p.rss_params.front_vehicle_deceleration > p.rss_params_for_abort.front_vehicle_deceleration || + p.rss_params.rear_vehicle_deceleration > p.rss_params_for_abort.rear_vehicle_deceleration || + p.rss_params.rear_vehicle_reaction_time > p.rss_params_for_abort.rear_vehicle_reaction_time || + p.rss_params.rear_vehicle_safety_time_margin > + p.rss_params_for_abort.rear_vehicle_safety_time_margin || + p.rss_params.lateral_distance_max_threshold > + p.rss_params_for_abort.lateral_distance_max_threshold || + p.rss_params.longitudinal_distance_min_threshold > + p.rss_params_for_abort.longitudinal_distance_min_threshold || + p.rss_params.longitudinal_velocity_delta_time > + p.rss_params_for_abort.longitudinal_velocity_delta_time) { + RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program..."); + exit(EXIT_FAILURE); + } + } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( logger_, "cancel.delta_time: " << p.cancel.delta_time From 0c48944c343cd74a2acb26cf428f113eb536a7f6 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sat, 7 Oct 2023 01:26:59 +0900 Subject: [PATCH 045/206] feat(intersection): yield initially on green light (#5234) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 4 + .../src/manager.cpp | 8 ++ .../src/scene_intersection.cpp | 76 ++++++++++++++++++- .../src/scene_intersection.hpp | 29 ++++--- 4 files changed, 103 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index e210d2fb21b88..972f572890d16 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -37,6 +37,10 @@ keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + yield_on_green_traffic_light: + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + range: 30.0 # [m] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3c96a171516bd..4137090a5b6ae 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -109,6 +109,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); ip.collision_detection.minimum_upstream_velocity = getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); + ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = + getOrDeclareParameter( + node, + ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); + ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); + ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.range"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b30a57f9e8dad..75bb4e861a60f 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,14 +30,15 @@ #include #include +#include #include #include #include +#include #include #include #include - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -807,6 +808,36 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +static bool isGreenSolidOn( + lanelet::ConstLanelet lane, const std::map & tl_infos) +{ + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + + std::optional tl_id = std::nullopt; + for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { + tl_id = tl_reg_elem->id(); + break; + } + if (!tl_id) { + // this lane has no traffic light + return false; + } + const auto tl_info_it = tl_infos.find(tl_id.value()); + if (tl_info_it == tl_infos.end()) { + // the info of this traffic light is not available + return false; + } + const auto & tl_info = tl_info_it->second; + for (auto && tl_light : tl_info.signal.elements) { + if ( + tl_light.color == TrafficSignalElement::GREEN && + tl_light.shape == TrafficSignalElement::CIRCLE) { + return true; + } + } + return false; +} + IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { @@ -994,13 +1025,52 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } + const auto target_objects = + filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + + // If there are any vehicles on the attention area when ego entered the intersection on green + // light, do pseudo collision detection because the vehicles are very slow and no collisions may + // be detected. check if ego vehicle entered assigned lanelet + const bool is_green_solid_on = + isGreenSolidOn(assigned_lanelet, planner_data_->traffic_light_id_map); + if (is_green_solid_on) { + if (!initial_green_light_observed_time_) { + const auto assigned_lane_begin_point = assigned_lanelet.centerline().front(); + const bool approached_assigned_lane = + motion_utils::calcSignedArcLength( + path->points, closest_idx, + tier4_autoware_utils::createPoint( + assigned_lane_begin_point.x(), assigned_lane_begin_point.y(), + assigned_lane_begin_point.z())) < + planner_param_.collision_detection.yield_on_green_traffic_light + .distance_to_assigned_lanelet_start; + if (approached_assigned_lane) { + initial_green_light_observed_time_ = clock_->now(); + } + } + if (initial_green_light_observed_time_) { + const auto now = clock_->now(); + const bool exist_close_vehicles = std::any_of( + target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { + return tier4_autoware_utils::calcDistance3d( + object.kinematics.initial_pose_with_covariance.pose, current_pose) < + planner_param_.collision_detection.yield_on_green_traffic_light.range; + }); + if ( + exist_close_vehicles && + rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < + planner_param_.collision_detection.yield_on_green_traffic_light.duration) { + return IntersectionModule::NonOccludedCollisionStop{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + } + } + // calculate dynamic collision around attention area const double time_to_restart = (is_go_out_ || is_prioritized) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( *path, target_objects, path_lanelets, closest_idx, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index b5cf3ec04cdc3..de7d97a50c133 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -96,6 +96,12 @@ class IntersectionModule : public SceneModuleInterface double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr bool use_upstream_velocity; double minimum_upstream_velocity; + struct YieldOnGreeTrafficLight + { + double distance_to_assigned_lanelet_start; + double duration; + double range; + } yield_on_green_traffic_light; } collision_detection; struct Occlusion { @@ -234,36 +240,37 @@ class IntersectionModule : public SceneModuleInterface const std::string turn_direction_; const bool has_traffic_light_; - bool is_go_out_ = false; - bool is_permanent_go_ = false; - DecisionResult prev_decision_result_; + bool is_go_out_{false}; + bool is_permanent_go_{false}; + DecisionResult prev_decision_result_{Indecisive{""}}; // Parameter PlannerParam planner_param_; - std::optional intersection_lanelets_; + std::optional intersection_lanelets_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_; - // OcclusionState prev_occlusion_state_ = OcclusionState::NONE; + std::optional> occlusion_attention_divisions_{std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; - // NOTE: uuid_ is base member + // for pseudo-collision detection when ego just entered intersection on green light and upcoming + // vehicles are very slow + std::optional initial_green_light_observed_time_{std::nullopt}; // for stuck vehicle detection const bool is_private_area_; // for RTC const UUID occlusion_uuid_; - bool occlusion_safety_ = true; - double occlusion_stop_distance_; - bool occlusion_activated_ = true; + bool occlusion_safety_{true}; + double occlusion_stop_distance_{0.0}; + bool occlusion_activated_{true}; // for first stop in two-phase stop - bool occlusion_first_stop_required_ = false; + bool occlusion_first_stop_required_{false}; void initializeRTCStatus(); void prepareRTCStatus( From 83821d4818cf83ea3bcb14511fc64482859ed7f1 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Sat, 7 Oct 2023 17:20:47 +0900 Subject: [PATCH 046/206] feat(rtc_auto_mode_manager): eliminate rtc auto mode manager (#5235) * change namespace of auto_mode Signed-off-by: kyoichi-sugahara * delete RTC auto mode manager package Signed-off-by: kyoichi-sugahara * delete rtc_replayer.param Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix * fix typo Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../launch/planning.launch.xml | 2 - .../scenario_planning/lane_driving.launch.xml | 5 - launch/tier4_planning_launch/package.xml | 1 - .../config/scene_module_manager.param.yaml | 20 ++-- .../config/blind_spot.param.yaml | 2 +- .../config/crosswalk.param.yaml | 2 +- .../config/detection_area.param.yaml | 2 +- .../config/intersection.param.yaml | 4 +- .../config/no_stopping_area.param.yaml | 2 +- .../config/traffic_light.param.yaml | 2 +- planning/rtc_auto_mode_manager/CMakeLists.txt | 29 ----- planning/rtc_auto_mode_manager/README.md | 47 -------- .../config/rtc_auto_mode_manager.param.yaml | 31 ----- .../include/rtc_auto_mode_manager/node.hpp | 44 ------- .../rtc_auto_mode_manager_interface.hpp | 53 --------- .../launch/rtc_auto_mode_manager.launch.xml | 7 -- planning/rtc_auto_mode_manager/package.xml | 28 ----- planning/rtc_auto_mode_manager/src/node.cpp | 51 -------- .../src/rtc_auto_mode_manager_interface.cpp | 109 ------------------ .../include/rtc_interface/rtc_interface.hpp | 2 +- .../launch/rtc_replayer.launch.xml | 6 +- 21 files changed, 19 insertions(+), 430 deletions(-) delete mode 100644 planning/rtc_auto_mode_manager/CMakeLists.txt delete mode 100644 planning/rtc_auto_mode_manager/README.md delete mode 100644 planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml delete mode 100644 planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp delete mode 100644 planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp delete mode 100644 planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml delete mode 100644 planning/rtc_auto_mode_manager/package.xml delete mode 100644 planning/rtc_auto_mode_manager/src/node.cpp delete mode 100644 planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 7e78c8842f7df..6ace3f423d1bc 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -15,8 +15,6 @@ - - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 24a33b9d3f3df..5f17256df6b5c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -14,11 +14,6 @@ - - - - - diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 39b95286bb6cc..5988d34cded88 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -68,7 +68,6 @@ obstacle_stop_planner planning_evaluator planning_validator - rtc_auto_mode_manager scenario_selector surround_obstacle_checker diff --git a/planning/behavior_path_planner/config/scene_module_manager.param.yaml b/planning/behavior_path_planner/config/scene_module_manager.param.yaml index 9895d9b5e473f..5797704e8a0ca 100644 --- a/planning/behavior_path_planner/config/scene_module_manager.param.yaml +++ b/planning/behavior_path_planner/config/scene_module_manager.param.yaml @@ -5,7 +5,7 @@ ros__parameters: external_request_lane_change_left: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -14,7 +14,7 @@ external_request_lane_change_right: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -23,7 +23,7 @@ lane_change_left: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -32,7 +32,7 @@ lane_change_right: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false @@ -41,7 +41,7 @@ start_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -50,7 +50,7 @@ side_shift: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -59,7 +59,7 @@ goal_planner: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: true @@ -68,7 +68,7 @@ avoidance: enable_module: true - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -77,7 +77,7 @@ avoidance_by_lc: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: false enable_simultaneous_execution_as_candidate_module: false keep_last: false @@ -86,7 +86,7 @@ dynamic_avoidance: enable_module: false - enable_rtc: true + enable_rtc: false enable_simultaneous_execution_as_approved_module: true enable_simultaneous_execution_as_candidate_module: true keep_last: false diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index 3aa8c82a5e556..f9ddd3ce61099 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,4 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index e7accc14096e0..a4f9e9d7ce23d 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -5,7 +5,7 @@ show_processing_time: false # [-] whether to show processing time # param for input data traffic_light_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - enable_rtc: true # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. + enable_rtc: false # if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. # param for stop position stop_position: diff --git a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml index b5d075a4d6493..62b5f2458336f 100644 --- a/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,4 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 972f572890d16..30fefcaeee035 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -66,8 +66,8 @@ attention_lane_curvature_calculation_ds: 0.5 enable_rtc: - intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval - intersection_to_occlusion: true + intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + intersection_to_occlusion: false merge_from_private: stop_line_margin: 3.0 diff --git a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml index f550188d4f2c1..c84848f8cc31d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml +++ b/planning/behavior_velocity_no_stopping_area_module/config/no_stopping_area.param.yaml @@ -8,4 +8,4 @@ stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area detection_area_length: 200.0 # [m] used to create detection area polygon stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml index e8e0357daa4a1..a837ae1b46b9b 100644 --- a/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml +++ b/planning/behavior_velocity_traffic_light_module/config/traffic_light.param.yaml @@ -5,4 +5,4 @@ tl_state_timeout: 1.0 yellow_lamp_period: 2.75 enable_pass_judge: true - enable_rtc: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/rtc_auto_mode_manager/CMakeLists.txt b/planning/rtc_auto_mode_manager/CMakeLists.txt deleted file mode 100644 index 52c0c68384a57..0000000000000 --- a/planning/rtc_auto_mode_manager/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(rtc_auto_mode_manager) - -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic -Werror) -endif() - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/rtc_auto_mode_manager_interface.cpp - src/node.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "rtc_auto_mode_manager::RTCAutoModeManagerNode" - EXECUTABLE ${PROJECT_NAME}_node -) - -ament_auto_package( - INSTALL_TO_SHARE - config - launch -) diff --git a/planning/rtc_auto_mode_manager/README.md b/planning/rtc_auto_mode_manager/README.md deleted file mode 100644 index 0e2614baaf1c8..0000000000000 --- a/planning/rtc_auto_mode_manager/README.md +++ /dev/null @@ -1,47 +0,0 @@ -# RTC Auto Mode Manager - -## Purpose - -RTC Auto Mode Manager is a node to approve request to cooperate from behavior planning modules automatically. - -## Inputs / Outputs - -### Input - -| Name | Type | Description | -| ------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -### Output - -| Name | Type | Description | -| ---------------------------------------- | --------------------------- | ------------------------------------------ | -| `/planning/enable_auto_mode/internal/**` | tier4_rtc_msgs/srv/AutoMode | Service to enable auto mode for the module | - -## Parameters - -| Name | Type | Description | -| :-------------------- | :--------------- | :----------------------------------------------- | -| `module_list` | List of `string` | Module names managing in `rtc_auto_mode_manager` | -| `default_enable_list` | List of `string` | Module names enabled auto mode at initialization | - -## Inner-workings / Algorithms - -```plantuml - -start -:Read parameters; -:Send enable auto mode service to the module listed in `default_enable_list`; -repeat - if (Enable auto mode command received?) then (yes) - :Send enable auto mode command to rtc_interface; - else (no) - endif -repeat while (Is node running?) is (yes) not (no) -end - -``` - -## Assumptions / Known limits - -## Future extensions / Unimplemented parts diff --git a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml b/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml deleted file mode 100644 index 0aa3cbd49e8e9..0000000000000 --- a/planning/rtc_auto_mode_manager/config/rtc_auto_mode_manager.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - module_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" - - default_enable_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "goal_planner" - - "start_planner" - - "intersection_occlusion" diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp deleted file mode 100644 index d36974508d2df..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/node.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__NODE_HPP_ -#define RTC_AUTO_MODE_MANAGER__NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status_array.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatusArray; -class RTCAutoModeManagerNode : public rclcpp::Node -{ -public: - explicit RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options); - -private: - rclcpp::TimerBase::SharedPtr timer_; - AutoModeStatusArray auto_mode_statuses_; - rclcpp::Publisher::SharedPtr statuses_pub_; - std::vector> managers_; -}; - -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__NODE_HPP_ diff --git a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp b/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp deleted file mode 100644 index 098852c397161..0000000000000 --- a/planning/rtc_auto_mode_manager/include/rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ -#define RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "tier4_rtc_msgs/msg/auto_mode_status.hpp" -#include "tier4_rtc_msgs/msg/module.hpp" -#include "tier4_rtc_msgs/srv/auto_mode.hpp" - -#include -#include -#include - -namespace rtc_auto_mode_manager -{ -using tier4_rtc_msgs::msg::AutoModeStatus; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::srv::AutoMode; - -class RTCAutoModeManagerInterface -{ -public: - RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable); - AutoModeStatus getAutoModeStatus() const { return auto_mode_status_; } - -private: - void onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response); - AutoMode::Request createRequest(const AutoMode::Request::SharedPtr request) const; - AutoModeStatus auto_mode_status_; - rclcpp::Client::SharedPtr enable_cli_; - rclcpp::Service::SharedPtr enable_srv_; - - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; -}; -} // namespace rtc_auto_mode_manager - -#endif // RTC_AUTO_MODE_MANAGER__RTC_AUTO_MODE_MANAGER_INTERFACE_HPP_ diff --git a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml b/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml deleted file mode 100644 index 85d4b3446fb89..0000000000000 --- a/planning/rtc_auto_mode_manager/launch/rtc_auto_mode_manager.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/planning/rtc_auto_mode_manager/package.xml b/planning/rtc_auto_mode_manager/package.xml deleted file mode 100644 index e9c16aac1bb91..0000000000000 --- a/planning/rtc_auto_mode_manager/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - rtc_auto_mode_manager - 0.1.0 - The rtc_auto_mode_manager package - - Fumiya Watanabe - Taiki Tanaka - - Apache License 2.0 - - Fumiya Watanabe - - ament_cmake_auto - autoware_cmake - - rclcpp - rclcpp_components - tier4_rtc_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning/rtc_auto_mode_manager/src/node.cpp b/planning/rtc_auto_mode_manager/src/node.cpp deleted file mode 100644 index 6092d2741d909..0000000000000 --- a/planning/rtc_auto_mode_manager/src/node.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/node.hpp" - -#include - -namespace rtc_auto_mode_manager -{ - -RTCAutoModeManagerNode::RTCAutoModeManagerNode(const rclcpp::NodeOptions & node_options) -: Node("rtc_auto_mode_manager_node", node_options) -{ - const std::vector module_list = - declare_parameter>("module_list"); - const std::vector default_enable_list = - declare_parameter>("default_enable_list"); - - for (const auto & module_name : module_list) { - const bool enabled = - std::count(default_enable_list.begin(), default_enable_list.end(), module_name) != 0; - managers_.push_back(std::make_shared(this, module_name, enabled)); - } - statuses_pub_ = create_publisher("output/auto_mode_statuses", 1); - using std::chrono_literals::operator""ms; - timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this] { - AutoModeStatusArray auto_mode_statuses; - for (const auto & m : managers_) { - auto_mode_statuses.stamp = get_clock()->now(); - auto_mode_statuses.statuses.emplace_back(m->getAutoModeStatus()); - } - statuses_pub_->publish(auto_mode_statuses); - auto_mode_statuses.statuses.clear(); - }); -} - -} // namespace rtc_auto_mode_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(rtc_auto_mode_manager::RTCAutoModeManagerNode) diff --git a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp b/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp deleted file mode 100644 index e28957cee161b..0000000000000 --- a/planning/rtc_auto_mode_manager/src/rtc_auto_mode_manager_interface.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "rtc_auto_mode_manager/rtc_auto_mode_manager_interface.hpp" - -namespace rtc_auto_mode_manager -{ -Module getModuleType(const std::string & module_name) -{ - Module module; - if (module_name == "blind_spot") { - module.type = Module::BLIND_SPOT; - } else if (module_name == "crosswalk") { - module.type = Module::CROSSWALK; - } else if (module_name == "detection_area") { - module.type = Module::DETECTION_AREA; - } else if (module_name == "intersection") { - module.type = Module::INTERSECTION; - } else if (module_name == "no_stopping_area") { - module.type = Module::NO_STOPPING_AREA; - } else if (module_name == "occlusion_spot") { - module.type = Module::OCCLUSION_SPOT; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "external_request_lane_change_left") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_LEFT; - } else if (module_name == "external_request_lane_change_right") { - module.type = Module::EXT_REQUEST_LANE_CHANGE_RIGHT; - } else if (module_name == "lane_change_left") { - module.type = Module::LANE_CHANGE_LEFT; - } else if (module_name == "lane_change_right") { - module.type = Module::LANE_CHANGE_RIGHT; - } else if (module_name == "avoidance_by_lane_change_left") { - module.type = Module::AVOIDANCE_BY_LC_LEFT; - } else if (module_name == "avoidance_by_lane_change_right") { - module.type = Module::AVOIDANCE_BY_LC_RIGHT; - } else if (module_name == "avoidance_left") { - module.type = Module::AVOIDANCE_LEFT; - } else if (module_name == "avoidance_right") { - module.type = Module::AVOIDANCE_RIGHT; - } else if (module_name == "goal_planner") { - module.type = Module::GOAL_PLANNER; - } else if (module_name == "start_planner") { - module.type = Module::START_PLANNER; - } else if (module_name == "intersection_occlusion") { - module.type = Module::INTERSECTION_OCCLUSION; - } else { - module.type = Module::NONE; - } - return module; -} - -RTCAutoModeManagerInterface::RTCAutoModeManagerInterface( - rclcpp::Node * node, const std::string & module_name, const bool default_enable) -{ - using std::chrono_literals::operator""s; - using std::placeholders::_1; - using std::placeholders::_2; - - // Service client - enable_cli_ = - node->create_client(enable_auto_mode_namespace_ + "/internal/" + module_name); - - while (!enable_cli_->wait_for_service(1s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(node->get_logger(), "Interrupted while waiting for service."); - rclcpp::shutdown(); - return; - } - RCLCPP_INFO_STREAM(node->get_logger(), "Waiting for service... [" << module_name << "]"); - } - - // Service - enable_srv_ = node->create_service( - enable_auto_mode_namespace_ + "/" + module_name, - std::bind(&RTCAutoModeManagerInterface::onEnableService, this, _1, _2)); - - auto_mode_status_.module = getModuleType(module_name); - // Send enable auto mode request - if (default_enable) { - auto_mode_status_.is_auto_mode = true; - AutoMode::Request::SharedPtr request = std::make_shared(); - request->enable = true; - enable_cli_->async_send_request(request); - } -} - -void RTCAutoModeManagerInterface::onEnableService( - const AutoMode::Request::SharedPtr request, const AutoMode::Response::SharedPtr response) -{ - auto_mode_status_.is_auto_mode = request->enable; - enable_cli_->async_send_request(request); - response->success = true; -} - -} // namespace rtc_auto_mode_manager diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 4929d0f8e27f3..d6e083421c0b7 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -85,7 +85,7 @@ class RTCInterface std::string cooperate_status_namespace_ = "/planning/cooperate_status"; std::string cooperate_commands_namespace_ = "/planning/cooperate_commands"; - std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode/internal"; + std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; mutable std::mutex mutex_; }; diff --git a/planning/rtc_replayer/launch/rtc_replayer.launch.xml b/planning/rtc_replayer/launch/rtc_replayer.launch.xml index 9b832b369c77c..70d7baf2dce4a 100644 --- a/planning/rtc_replayer/launch/rtc_replayer.launch.xml +++ b/planning/rtc_replayer/launch/rtc_replayer.launch.xml @@ -1,7 +1,3 @@ - - - - - + From c508aa71c19d09f4198e24cd93c39191c9dacca6 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 9 Oct 2023 11:29:00 +0900 Subject: [PATCH 047/206] fix(image_projection_based_fusion): add iou_x use in long range for roi_cluster_fusion (#5148) * fix: add iou_x for long range obj Signed-off-by: badai-nguyen * fix: add launch file param Signed-off-by: badai-nguyen * chore: fix unexpect calc iou in long range Signed-off-by: badai-nguyen * fix: multi iou usable Signed-off-by: badai-nguyen * chore: typo Signed-off-by: badai-nguyen * docs: update readme Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...ra_lidar_fusion_based_detection.launch.xml | 6 +- ...ar_radar_fusion_based_detection.launch.xml | 6 +- .../detection/detection.launch.xml | 6 +- .../launch/perception.launch.xml | 6 +- .../docs/roi-cluster-fusion.md | 26 ++++---- .../roi_cluster_fusion/node.hpp | 18 +++--- .../launch/roi_cluster_fusion.launch.xml | 13 ++-- .../src/roi_cluster_fusion/node.cpp | 59 ++++++++++++------- 8 files changed, 86 insertions(+), 54 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 51359d8d8f0e2..67e123ea5b018 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -15,7 +15,8 @@ - + + @@ -169,7 +170,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 45cfb91bf25c1..fbe5f21c041b6 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -15,7 +15,8 @@ - + + @@ -190,7 +191,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 77a3e345ec5e8..1fcf29606891b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -35,7 +35,8 @@ - + + @@ -64,7 +65,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 3d4ce1035e72c..bbb7ebb2d8d25 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -93,7 +93,8 @@ - + + @@ -169,7 +170,8 @@ - + + diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 01cdc3756866b..03eaab2a3c6ca 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -32,18 +32,20 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ### Core Parameters -| Name | Type | Description | -| --------------------------- | ----- | -------------------------------------------------------------------------------- | -| `use_iou_x` | bool | calculate IoU only along x-axis | -| `use_iou_y` | bool | calculate IoU only along y-axis | -| `use_iou` | bool | calculate IoU both along x-axis and y-axis | -| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | -| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | -| `roi_scale_factor` | float | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | -| `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | -| `unknown_iou_threshold` | float | the IoU threshold to fuse cluster with unknown label of roi | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | +| Name | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `fusion_distance` | double | If the detected object's distance to frame_id is less than the threshold, the fusion will be processed | +| `trust_object_distance` | double | if the detected object's distance is less than the `trust_object_distance`, `trust_object_iou_mode` will be used, otherwise `non_trust_object_iou_mode` will be used | +| `trust_object_iou_mode` | string | select mode from 3 options {`iou`, `iou_x`, `iou_y`} to calculate IoU in range of [`0`, `trust_distance`].
 `iou`: IoU along x-axis and y-axis
 `iou_x`: IoU along x-axis
 `iou_y`: IoU along y-axis | +| `non_trust_object_iou_mode` | string | the IOU mode using in range of [`trust_distance`, `fusion_distance`] if `trust_distance` < `fusion_distance` | +| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | +| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | +| `roi_scale_factor` | double | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | +| `iou_threshold` | double | the IoU threshold to overwrite a label of clusters with a label of roi | +| `unknown_iou_threshold` | double | the IoU threshold to fuse cluster with unknown label of roi | +| `remove_unknown` | bool | if `true`, remove all `UNKNOWN` labeled objects from output | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 70a7866e79b87..e54710ad477da 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -17,10 +17,12 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include #include - +#include namespace image_projection_based_fusion { +const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode : public FusionNode @@ -38,9 +40,7 @@ class RoiClusterFusionNode const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) override; - bool use_iou_x_{false}; - bool use_iou_y_{false}; - bool use_iou_{false}; + std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; double roi_scale_factor_{1.1}; @@ -49,10 +49,14 @@ class RoiClusterFusionNode const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - double trust_distance_; - - bool filter_by_distance(const DetectedObjectWithFeature & obj); + double fusion_distance_; + double trust_object_distance_; + std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj); + double cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 2d99c25bb68f7..60f6f943b8cda 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -1,5 +1,5 @@ - + @@ -20,7 +20,8 @@ - + + @@ -45,9 +46,8 @@ - - - + + @@ -55,7 +55,8 @@ - + + diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 316d29dae1301..4a46c8aace696 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x"); - use_iou_y_ = declare_parameter("use_iou_y"); - use_iou_ = declare_parameter("use_iou"); + trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); + non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); roi_scale_factor_ = declare_parameter("roi_scale_factor"); iou_threshold_ = declare_parameter("iou_threshold"); unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); remove_unknown_ = declare_parameter("remove_unknown"); - trust_distance_ = declare_parameter("trust_distance"); + fusion_distance_ = declare_parameter("fusion_distance"); + trust_object_distance_ = declare_parameter("trust_object_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -111,7 +111,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { continue; } @@ -175,25 +175,23 @@ void RoiClusterFusionNode::fuseOnSingleImage( bool is_roi_label_known = feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { - double iou(0.0), iou_x(0.0), iou_y(0.0); - if (use_iou_) { - iou = calcIoU(cluster_map.second, feature_obj.feature.roi); - } - // use for unknown roi to improve small objects like traffic cone detect - // TODO(badai-nguyen): add option to disable roi_cluster mode - if (use_iou_x_ || !is_roi_label_known) { - iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); - } - if (use_iou_y_) { - iou_y = calcIoUY(cluster_map.second, feature_obj.feature.roi); + double iou(0.0); + bool is_use_non_trust_object_iou_mode = is_far_enough( + input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); + if (is_use_non_trust_object_iou_mode || is_roi_label_known) { + iou = + cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_); + } else { + iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_); } + const bool passed_inside_cluster_gate = only_allow_inside_cluster_ ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_) : true; - if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) { + if (max_iou < iou && passed_inside_cluster_gate) { index = cluster_map.first; - max_iou = iou + iou_x + iou_y; + max_iou = iou; associated = true; } } @@ -274,11 +272,30 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } -bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +bool RoiClusterFusionNode::is_far_enough( + const DetectedObjectWithFeature & obj, const double distance_threshold) { const auto & position = obj.object.kinematics.pose_with_covariance.pose.position; - const auto square_distance = position.x * position.x + position.y + position.y; - return square_distance > trust_distance_ * trust_distance_; + return position.x * position.x + position.y * position.y > + distance_threshold * distance_threshold; +} + +double RoiClusterFusionNode::cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode) +{ + switch (IOU_MODE_MAP.at(iou_mode)) { + case 0 /* use iou mode */: + return calcIoU(roi_1, roi_2); + + case 1 /* use iou_x mode */: + return calcIoUX(roi_1, roi_2); + + case 2 /* use iou_y mode */: + return calcIoUY(roi_1, roi_2); + default: + return 0.0; + } } } // namespace image_projection_based_fusion From 9ddb07ae06c269d8cf007bab8fe402c24de54223 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 10 Oct 2023 14:06:17 +0900 Subject: [PATCH 048/206] fix(vehicle_cmd_gate): fix filtering condition (#5250) Signed-off-by: kminoda --- control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index e2ee0ec65eec0..e681bc22cd24b 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -555,9 +555,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont // set prev value for both to keep consistency over switching: // Actual steer, vel, acc should be considered in manual mode to prevent sudden motion when // switching from manual to autonomous - const auto in_autonomous = - (mode.mode == OperationModeState::AUTONOMOUS && mode.is_autoware_control_enabled); - auto prev_values = in_autonomous ? out : current_status_cmd; + auto prev_values = mode.is_autoware_control_enabled ? out : current_status_cmd; if (ego_is_stopped) { prev_values.longitudinal = out.longitudinal; From adb4872de9bedac1ff7440b135e741d5da2336ae Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 10 Oct 2023 18:01:13 +0900 Subject: [PATCH 049/206] fix(lane_change): add guard to neigibor lanelets (#5245) Signed-off-by: kosuke55 --- .../src/scene_module/lane_change/normal.cpp | 3 +++ planning/behavior_path_planner/src/utils/lane_change/utils.cpp | 3 +++ 2 files changed, 6 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c75970e6fcc2d..71ba8c51ec6ad 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -946,6 +946,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + return false; + } const auto target_objects = getTargetObjects(current_lanes, target_lanes); debug_filtered_objects_ = target_objects; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index df609dce48512..038cf9488345f 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -236,6 +236,9 @@ lanelet::BasicPolygon2d getTargetNeighborLanesPolygon( { const auto target_neighbor_lanelets = utils::lane_change::getTargetNeighborLanes(route_handler, current_lanes, type); + if (target_neighbor_lanelets.empty()) { + return {}; + } const auto target_neighbor_preferred_lane_poly = lanelet::utils::getPolygonFromArcLength( target_neighbor_lanelets, 0, std::numeric_limits::max()); From b36bc09b982f3f7b95c7c5164fa436b5b313ca00 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Tue, 10 Oct 2023 18:31:52 +0900 Subject: [PATCH 050/206] chore(default_ad_api_helpers): update readme topic (#5258) * chore(default_ad_api_helpers): update readme topic Signed-off-by: kminoda * style(pre-commit): autofix * update readme Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- system/default_ad_api_helpers/ad_api_adaptors/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/default_ad_api_helpers/ad_api_adaptors/README.md index 121befb68c88a..2e299fd7a2e51 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/default_ad_api_helpers/ad_api_adaptors/README.md @@ -21,9 +21,11 @@ The clear API is called automatically before setting the route. | Interface | Local Name | Global Name | Description | | ------------ | ------------------ | ------------------------------------- | -------------------------------------------------- | +| Subscription | - | /api/routing/state | The state of the routing API. | | Subscription | ~/input/fixed_goal | /planning/mission_planning/goal | The goal pose of route. Disable goal modification. | | Subscription | ~/input/rough_goal | /rviz/routing/rough_goal | The goal pose of route. Enable goal modification. | | Subscription | ~/input/reroute | /rviz/routing/reroute | The goal pose of reroute. | | Subscription | ~/input/waypoint | /planning/mission_planning/checkpoint | The waypoint pose of route. | | Client | - | /api/routing/clear_route | The route clear API. | | Client | - | /api/routing/set_route_points | The route points set API. | +| Client | - | /api/routing/change_route_points | The route points change API. | From 45339f63a05434cceaabbb2fa2d162b96e11d275 Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Tue, 10 Oct 2023 18:16:58 +0800 Subject: [PATCH 051/206] feat(autoware_auto_msgs_adapter): add map adapter (#4700) * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * feat(autoware_auto_msgs_adapter): add map adapter Signed-off-by: Cynthia Liu * style(pre-commit): autofix --------- Signed-off-by: Cynthia Liu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autoware_auto_msgs_adapter/CMakeLists.txt | 1 + .../config/adapter_map.param.yaml | 5 + .../autoware_auto_msgs_adapter.launch.xml | 4 + system/autoware_auto_msgs_adapter/package.xml | 4 +- .../autoware_auto_msgs_adapter.schema.json | 1 + .../src/autoware_auto_msgs_adapter_core.cpp | 5 + .../src/include/adapter_map.hpp | 59 ++++++++++ .../autoware_auto_msgs_adapter_core.hpp | 1 + .../test/test_msg_had_map_bin.cpp | 104 ++++++++++++++++++ 9 files changed, 182 insertions(+), 2 deletions(-) create mode 100644 system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml create mode 100644 system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp create mode 100644 system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt index ff8dcf6cedbb3..8b1d31ff2d01b 100644 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${NODE_NAME} src/include/adapter_base.hpp src/include/adapter_base_interface.hpp src/include/adapter_control.hpp + src/include/adapter_map.hpp src/include/adapter_perception.hpp src/include/autoware_auto_msgs_adapter_core.hpp src/autoware_auto_msgs_adapter_core.cpp) diff --git a/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml new file mode 100644 index 0000000000000..dcaa49e089b44 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_map.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_mapping_msgs/msg/HADMapBin" + topic_name_source: "/map/vector_map" + topic_name_target: "/map/vector_map_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml index f914fe6e824c4..d80c1bd8cbedf 100755 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -1,11 +1,15 @@ + + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml index de19032e1aee9..99a94fa043565 100644 --- a/system/autoware_auto_msgs_adapter/package.xml +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -10,8 +10,6 @@ Apache License 2.0 - M. Fatih Cırıt - ament_cmake_auto autoware_cmake @@ -22,8 +20,10 @@ autoware_lint_common autoware_auto_control_msgs + autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_control_msgs + autoware_map_msgs autoware_perception_msgs rclcpp rclcpp_components diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json index 35e3aef1511cc..f6aa87f774528 100644 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json @@ -11,6 +11,7 @@ "description": "Target message type", "enum": [ "autoware_auto_control_msgs/msg/AckermannControlCommand", + "autoware_auto_mapping_msgs/msg/HADMapBin", "autoware_auto_perception_msgs/msg/PredictedObjects" ], "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp index 0f65571aba07b..15e3c90d227dd 100644 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp @@ -57,6 +57,11 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( return std::static_pointer_cast( std::make_shared(*this, topic_name_source, topic_name_target)); }}, + {"autoware_auto_mapping_msgs/msg/HADMapBin", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, {"autoware_auto_perception_msgs/msg/PredictedObjects", [&] { return std::static_pointer_cast( diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp new file mode 100644 index 0000000000000..8150b7d7dba08 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_map.hpp @@ -0,0 +1,59 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#ifndef ADAPTER_MAP_HPP_ +#define ADAPTER_MAP_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; + +class AdapterMap : public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterMap( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterMap is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + HADMapBin convert(const LaneletMapBin & msg_source) override + { + autoware_auto_mapping_msgs::msg::HADMapBin msg_auto; + msg_auto.header = msg_source.header; + msg_auto.format_version = msg_source.version_map_format; + msg_auto.map_version = msg_source.version_map; + msg_auto.map_format = 0; + msg_auto.data = msg_source.data; + + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_MAP_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp index f595359dcc1e7..f0e28f48aacd8 100644 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp @@ -15,6 +15,7 @@ #define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ #include "adapter_control.hpp" +#include "adapter_map.hpp" #include "adapter_perception.hpp" #include diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp new file mode 100644 index 0000000000000..b312bd144f6f3 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_had_map_bin.cpp @@ -0,0 +1,104 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include + +autoware_map_msgs::msg::LaneletMapBin generate_map_msg() +{ + // generate deterministic random int + std::mt19937 gen(0); + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_map_msgs::msg::LaneletMapBin msg_map; + msg_map.header.stamp = rclcpp::Time(rand_int()); + msg_map.header.frame_id = "test_frame"; + + msg_map.version_map_format = "1.1.1"; + msg_map.version_map = "1.0.0"; + msg_map.name_map = "florence-prato-city-center"; + msg_map.data.push_back(rand_int()); + + return msg_map; +} + +TEST(AutowareAutoMsgsAdapter, TestHADMapBin) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_mapping_msgs/msg/HADMapBin"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_map = generate_map_msg(); + auto sub = node_subscriber->create_subscription( + topic_name_target, rclcpp::QoS{1}, + [&msg_map, &test_completed](const autoware_auto_mapping_msgs::msg::HADMapBin::SharedPtr msg) { + EXPECT_EQ(msg->header.stamp, msg_map.header.stamp); + EXPECT_EQ(msg->header.frame_id, msg_map.header.frame_id); + + EXPECT_EQ(msg->map_format, 0); + EXPECT_EQ(msg->format_version, msg_map.version_map_format); + EXPECT_EQ(msg->map_version, msg_map.version_map); + EXPECT_EQ(msg->data, msg_map.data); + + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_map); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + // rclcpp::shutdown(); +} From 82d9840bc16dd380e2509e4fdac6d0be041f1796 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Oct 2023 09:05:36 +0900 Subject: [PATCH 052/206] refactor(behavior_path_planner): separate processing time function (#5256) Signed-off-by: Takamasa Horibe --- .../include/behavior_path_planner/planner_manager.hpp | 5 +++++ .../src/behavior_path_planner_node.cpp | 1 + planning/behavior_path_planner/src/planner_manager.cpp | 10 ++++++++-- 3 files changed, 14 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index ee5771c98217e..72c32039da627 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -242,6 +242,11 @@ class PlannerManager */ void print() const; + /** + * @brief publish processing time of each module. + */ + void publishProcessingTime() const; + /** * @brief visit each module and get debug information. */ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 6e37bd0ec4b2c..755817aa22ae7 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -579,6 +579,7 @@ void BehaviorPathPlannerNode::run() lk_pd.unlock(); // release planner_data_ planner_manager_->print(); + planner_manager_->publishProcessingTime(); planner_manager_->publishMarker(); planner_manager_->publishVirtualWall(); lk_manager.unlock(); // release planner_manager_ diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 5963ddb778874..24967608775f3 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -886,13 +886,19 @@ void PlannerManager::print() const string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first << ":" << std::setw(4) << std::right << t.second << "ms]\n" << std::setw(21); - std::string name = std::string("processing_time/") + t.first; - debug_publisher_ptr_->publish(name, t.second); } RCLCPP_INFO_STREAM(logger_, string_stream.str()); } +void PlannerManager::publishProcessingTime() const +{ + for (const auto & t : processing_time_) { + std::string name = std::string("processing_time/") + t.first; + debug_publisher_ptr_->publish(name, t.second); + } +} + std::shared_ptr PlannerManager::getDebugMsg() { debug_msg_ptr_ = std::make_shared(); From 2aa9d1cd68b81df7b154195e7243f3e872256b8e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 11 Oct 2023 09:05:54 +0900 Subject: [PATCH 053/206] chore(obstacle_velocity_limiter): publish processing time as float (#5257) Signed-off-by: Takamasa Horibe --- planning/obstacle_velocity_limiter/README.md | 2 +- .../obstacle_velocity_limiter_node.hpp | 4 ++-- .../src/obstacle_velocity_limiter_node.cpp | 6 ++++-- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index d670f07d26e9d..36aa35bb31156 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -161,7 +161,7 @@ For example a value of `1` means all trajectory points will be evaluated while a | ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | | `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | | `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `std_msgs/Int64` | Time taken to calculate the trajectory (in microseconds) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index a2ff72d0da562..cbdd390183d61 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -29,7 +29,7 @@ #include #include #include -#include +#include #include #include @@ -56,7 +56,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node pub_trajectory_; //!< @brief publisher for output trajectory rclcpp::Publisher::SharedPtr pub_debug_markers_; //!< @brief publisher for debug markers - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_runtime_; //!< @brief publisher for callback runtime rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief subscriber for reference trajectory diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index f4601979b9b8a..086d76e9ef0fe 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -68,7 +68,8 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio pub_trajectory_ = create_publisher("~/output/trajectory", 1); pub_debug_markers_ = create_publisher("~/output/debug_markers", 1); - pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); + pub_runtime_ = + create_publisher("~/output/runtime_microseconds", 1); const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -223,7 +224,8 @@ void ObstacleVelocityLimiterNode::onTrajectory(const Trajectory::ConstSharedPtr const auto t_end = std::chrono::system_clock::now(); const auto runtime = std::chrono::duration_cast(t_end - t_start); - pub_runtime_->publish(std_msgs::msg::Int64().set__data(runtime.count())); + pub_runtime_->publish(tier4_debug_msgs::msg::Float64Stamped().set__stamp(now()).set__data( + static_cast(runtime.count()))); if (pub_debug_markers_->get_subscription_count() > 0) { const auto safe_projected_linestrings = From 3a8ff49eb0da121c6c0a28546f0853d5853f57d4 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Wed, 11 Oct 2023 10:09:48 +0900 Subject: [PATCH 054/206] fix(map_projection_loader): fix sample config parameter in readme (#5262) fix readme Signed-off-by: Kento Yabuuchi --- map/map_projection_loader/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 0b0c41821d036..94b142da3dcf9 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -25,7 +25,7 @@ sample-map-rosbag ```yaml # map_projector_info.yaml -type: "local" +projector_type: local ``` ### Using MGRS From 671cc9658f8ccf52de03089f7b1ddc9e6c59a2a5 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 11 Oct 2023 15:58:07 +0900 Subject: [PATCH 055/206] feat(intersection): find stop line from footprint intersection with areas (#5264) Signed-off-by: Mamoru Sobue --- .../src/scene_intersection.cpp | 3 +- .../src/scene_merge_from_private_road.cpp | 6 +- .../src/util.cpp | 106 +++++++++++++----- .../src/util_type.hpp | 6 +- 4 files changed, 87 insertions(+), 34 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 75bb4e861a60f..f5eda0d0fe4d7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -877,7 +877,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); const bool is_prioritized = traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; - intersection_lanelets.update(is_prioritized, interpolated_path_info); + const auto footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(is_prioritized, interpolated_path_info, footprint); // this is abnormal const auto & conflicting_lanelets = intersection_lanelets.conflicting(); diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index bb54d829cc477..698242da67528 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -94,8 +94,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR planner_param_.attention_area_length, planner_param_.occlusion_attention_area_length, planner_param_.consider_wrong_direction_vehicle); } - intersection_lanelets_.value().update(false, interpolated_path_info); - const auto & first_conflicting_area = intersection_lanelets_.value().first_conflicting_area(); + auto & intersection_lanelets = intersection_lanelets_.value(); + const auto local_footprint = planner_data_->vehicle_info_.createFootprint(0.0, 0.0); + intersection_lanelets.update(false, interpolated_path_info, local_footprint); + const auto & first_conflicting_area = intersection_lanelets.first_conflicting_area(); if (!first_conflicting_area) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 2f3fcc6f6fde0..7862a533f34eb 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -220,6 +220,50 @@ static std::optional getStopLineIndexFromMap( return stop_idx_ip_opt.get(); } +static std::optional getFirstPointInsidePolygonByFootprint( + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (auto i = lane_start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = tier4_autoware_utils::transformVector( + footprint, tier4_autoware_utils::pose2transform(base_pose)); + if (bg::intersects(path_footprint, area_2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + +static std::optional> +getFirstPointInsidePolygonsByFootprint( + const std::vector & polygons, + const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + + for (size_t i = lane_start; i <= lane_end; ++i) { + const auto & pose = path_ip.points.at(i).point.pose; + const auto path_footprint = + tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); + for (const auto & polygon : polygons) { + const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + const bool is_in_polygon = bg::intersects(area_2d, path_footprint); + if (is_in_polygon) { + return std::make_optional>( + i, polygon); + } + } + } + return std::nullopt; +} + std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::CompoundPolygon3d & first_detection_area, @@ -236,6 +280,7 @@ std::optional generateIntersectionStopLines( const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); + /* // find the index of the first point that intersects with detection_areas const auto first_inside_detection_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_detection_area); @@ -244,6 +289,16 @@ std::optional generateIntersectionStopLines( return std::nullopt; } const auto first_inside_detection_ip = first_inside_detection_idx_ip_opt.value(); + */ + // find the index of the first point whose vehicle footprint on it intersects with detection_area + const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); + std::optional first_footprint_inside_detection_ip_opt = + getFirstPointInsidePolygonByFootprint( + first_detection_area, interpolated_path_info, local_footprint); + if (!first_footprint_inside_detection_ip_opt) { + return std::nullopt; + } + const auto first_footprint_inside_detection_ip = first_footprint_inside_detection_ip_opt.value(); // (1) default stop line position on interpolated path bool default_stop_line_valid = true; @@ -254,8 +309,7 @@ std::optional generateIntersectionStopLines( stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = static_cast(first_inside_detection_ip) - stop_line_margin_idx_dist - - base2front_idx_dist; + stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist; } if (stop_idx_ip_int < 0) { default_stop_line_valid = false; @@ -272,8 +326,6 @@ std::optional generateIntersectionStopLines( const auto closest_idx_ip = closest_idx_ip_opt.value(); // (3) occlusion peeking stop line position on interpolated path - const auto local_footprint = planner_data->vehicle_info_.createFootprint(0.0, 0.0); - const auto area_2d = lanelet::utils::to2D(first_detection_area).basicPolygon(); int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); bool occlusion_peeking_line_valid = true; { @@ -281,20 +333,13 @@ std::optional generateIntersectionStopLines( const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); - if (bg::intersects(path_footprint0, area_2d)) { + if (bg::intersects( + path_footprint0, lanelet::utils::to2D(first_detection_area).basicPolygon())) { occlusion_peeking_line_valid = false; } } if (occlusion_peeking_line_valid) { - for (size_t i = default_stop_line_ip + 1; i <= std::get<1>(lane_interval_ip); ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = tier4_autoware_utils::transformVector( - local_footprint, tier4_autoware_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, area_2d)) { - occlusion_peeking_line_ip_int = i; - break; - } - } + occlusion_peeking_line_ip_int = first_footprint_inside_detection_ip; } const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; @@ -310,8 +355,8 @@ std::optional generateIntersectionStopLines( const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); - int pass_judge_ip_int = static_cast(first_inside_detection_ip) - base2front_idx_dist - - std::ceil(braking_dist / ds); + int pass_judge_ip_int = + static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( std::clamp(pass_judge_ip_int, 0, static_cast(path_ip.points.size()) - 1)); @@ -321,18 +366,18 @@ std::optional generateIntersectionStopLines( if (use_stuck_stopline) { // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. - const auto stuck_stop_line_idx_ip_opt = - getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_conflicting_area); + const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + first_conflicting_area, interpolated_path_info, local_footprint); if (!stuck_stop_line_idx_ip_opt) { stuck_stop_line_valid = false; stuck_stop_line_ip_int = 0; } else { - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value(); + stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist; } } else { - stuck_stop_line_ip_int = std::get<0>(lane_interval_ip); + stuck_stop_line_ip_int = + std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist); } - stuck_stop_line_ip_int -= (stop_line_margin_idx_dist + base2front_idx_dist); if (stuck_stop_line_ip_int < 0) { stuck_stop_line_valid = false; } @@ -1243,21 +1288,22 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info) + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint) { is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path - const auto & path = interpolated_path_info.path; - const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); - { - auto first = getFirstPointInsidePolygons(path, lane_interval, conflicting_area_); - if (first && !first_conflicting_area_) { + if (!first_conflicting_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); + if (first) { first_conflicting_area_ = first.value().second; } } - { - auto first = getFirstPointInsidePolygons(path, lane_interval, attention_area_); - if (first && !first_attention_area_) { + if (!first_attention_area_) { + auto first = + getFirstPointInsidePolygonsByFootprint(attention_area_, interpolated_path_info, footprint); + if (first) { first_attention_area_ = first.value().second; } } diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index aed81d771e480..581f22904fd2b 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -15,6 +15,8 @@ #ifndef UTIL_TYPE_HPP_ #define UTIL_TYPE_HPP_ +#include + #include #include #include @@ -67,7 +69,9 @@ struct InterpolatedPathInfo struct IntersectionLanelets { public: - void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info); + void update( + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info, + const tier4_autoware_utils::LinearRing2d & footprint); const lanelet::ConstLanelets & attention() const { return is_prioritized_ ? attention_non_preceding_ : attention_; From 9fe7626889668b80298c7aeeba3068b6b622fc73 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 11 Oct 2023 15:58:22 +0900 Subject: [PATCH 056/206] fix(intersection): collision check considering object width (#5265) Signed-off-by: Takayuki Murooka --- .../src/scene_intersection.cpp | 35 ++++++++++++++++--- 1 file changed, 31 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index f5eda0d0fe4d7..fb35834ab39a5 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -43,6 +44,32 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +namespace +{ +Polygon2d createOneStepPolygon( + const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, + const autoware_auto_perception_msgs::msg::Shape & shape) +{ + const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); + const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); + + Polygon2d one_step_poly; + for (const auto & point : prev_poly.outer()) { + one_step_poly.outer().push_back(point); + } + for (const auto & point : next_poly.outer()) { + one_step_poly.outer().push_back(point); + } + + bg::correct(one_step_poly); + + Polygon2d convex_one_step_poly; + bg::convex_hull(one_step_poly, convex_one_step_poly); + + return convex_one_step_poly; +} +} // namespace + static bool isTargetCollisionVehicleType( const autoware_auto_perception_msgs::msg::PredictedObject & object) { @@ -1313,14 +1340,14 @@ bool IntersectionModule::checkCollision( // collision point const auto first_itr = std::adjacent_find( predicted_path.path.cbegin(), predicted_path.path.cend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (first_itr == predicted_path.path.cend()) continue; const auto last_itr = std::adjacent_find( predicted_path.path.crbegin(), predicted_path.path.crend(), - [&ego_poly](const auto & a, const auto & b) { - return bg::intersects(ego_poly, LineString2d{to_bg2d(a), to_bg2d(b)}); + [&ego_poly, &object](const auto & a, const auto & b) { + return bg::intersects(ego_poly, createOneStepPolygon(a, b, object.shape)); }); if (last_itr == predicted_path.path.crend()) continue; From dc8cc07bfebeed7aef328891a720bfc17899726d Mon Sep 17 00:00:00 2001 From: cyn-liu <104069308+cyn-liu@users.noreply.github.com> Date: Wed, 11 Oct 2023 15:48:44 +0800 Subject: [PATCH 057/206] feat(kalman_filter): add gtest (#4799) * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * feat(kalman_filter): add gtest Signed-off-by: Cynthia Liu * style(pre-commit): autofix --------- Signed-off-by: Cynthia Liu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/kalman_filter/CMakeLists.txt | 7 + common/kalman_filter/package.xml | 2 + .../kalman_filter/test/test_kalman_filter.cpp | 94 ++++++++++++++ .../test/test_time_delay_kalman_filter.cpp | 121 ++++++++++++++++++ 4 files changed, 224 insertions(+) create mode 100644 common/kalman_filter/test/test_kalman_filter.cpp create mode 100644 common/kalman_filter/test/test_time_delay_kalman_filter.cpp diff --git a/common/kalman_filter/CMakeLists.txt b/common/kalman_filter/CMakeLists.txt index dd59da727605d..6cbdf67a0affa 100644 --- a/common/kalman_filter/CMakeLists.txt +++ b/common/kalman_filter/CMakeLists.txt @@ -19,4 +19,11 @@ ament_auto_add_library(kalman_filter SHARED include/kalman_filter/time_delay_kalman_filter.hpp ) +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_add_ros_isolated_gtest(test_kalman_filter ${test_files}) + + target_link_libraries(test_kalman_filter kalman_filter) +endif() + ament_auto_package() diff --git a/common/kalman_filter/package.xml b/common/kalman_filter/package.xml index ea061f3f23bb8..200440b5774c7 100644 --- a/common/kalman_filter/package.xml +++ b/common/kalman_filter/package.xml @@ -18,7 +18,9 @@ eigen3_cmake_module ament_cmake_cppcheck + ament_cmake_ros ament_lint_auto + autoware_lint_common ament_cmake diff --git a/common/kalman_filter/test/test_kalman_filter.cpp b/common/kalman_filter/test/test_kalman_filter.cpp new file mode 100644 index 0000000000000..4f4e9ce44669b --- /dev/null +++ b/common/kalman_filter/test/test_kalman_filter.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "kalman_filter/kalman_filter.hpp" + +#include + +TEST(kalman_filter, kf) +{ + KalmanFilter kf_; + + Eigen::MatrixXd x_t(2, 1); + x_t << 1, 2; + + Eigen::MatrixXd P_t(2, 2); + P_t << 1, 0, 0, 1; + + Eigen::MatrixXd Q_t(2, 2); + Q_t << 0.01, 0, 0, 0.01; + + Eigen::MatrixXd R_t(2, 2); + R_t << 0.09, 0, 0, 0.09; + + Eigen::MatrixXd C_t(2, 2); + C_t << 1, 0, 0, 1; + + Eigen::MatrixXd A_t(2, 2); + A_t << 1, 0, 0, 1; + + Eigen::MatrixXd B_t(2, 2); + B_t << 1, 0, 0, 1; + + // Initialize the filter and check if initialization was successful + EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); + + // Perform prediction + Eigen::MatrixXd u_t(2, 1); + u_t << 0.1, 0.1; + EXPECT_TRUE(kf_.predict(u_t)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; + Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; + + Eigen::MatrixXd x_predict; + kf_.getX(x_predict); + Eigen::MatrixXd P_predict; + kf_.getP(P_predict); + + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + + // Perform update + Eigen::MatrixXd y_t(2, 1); + y_t << 1.05, 2.05; + EXPECT_TRUE(kf_.update(y_t)); + + // Check the updated state and covariance matrix + const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_t * x_predict_expected; + Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); + Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); + + Eigen::MatrixXd x_update; + kf_.getX(x_update); + Eigen::MatrixXd P_update; + kf_.getP(P_update); + + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); +} + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + bool result = RUN_ALL_TESTS(); + return result; +} diff --git a/common/kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp new file mode 100644 index 0000000000000..32fefd8ceff70 --- /dev/null +++ b/common/kalman_filter/test/test_time_delay_kalman_filter.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "kalman_filter/time_delay_kalman_filter.hpp" + +#include + +TEST(time_delay_kalman_filter, td_kf) +{ + TimeDelayKalmanFilter td_kf_; + + Eigen::MatrixXd x_t(3, 1); + x_t << 1.0, 2.0, 3.0; + Eigen::MatrixXd P_t(3, 3); + P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; + const int max_delay_step = 5; + const int dim_x = x_t.rows(); + const int dim_x_ex = dim_x * max_delay_step; + // Initialize the filter + td_kf_.init(x_t, P_t, max_delay_step); + + // Check if initialization was successful + Eigen::MatrixXd x_init = td_kf_.getLatestX(); + Eigen::MatrixXd P_init = td_kf_.getLatestP(); + Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); + Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + for (int i = 0; i < max_delay_step; ++i) { + x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; + P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; + } + + EXPECT_EQ(x_init.rows(), 3); + EXPECT_EQ(x_init.cols(), 1); + EXPECT_EQ(P_init.rows(), 3); + EXPECT_EQ(P_init.cols(), 3); + EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); + EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); + EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); + EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); + EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); + EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); + + // Define prediction parameters + Eigen::MatrixXd A_t(3, 3); + A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; + Eigen::MatrixXd Q_t(3, 3); + Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; + Eigen::MatrixXd x_next(3, 1); + x_next << 2.0, 4.0, 6.0; + + // Perform prediction + EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); + + // Check the prediction state and covariance matrix + Eigen::MatrixXd x_predict = td_kf_.getLatestX(); + Eigen::MatrixXd P_predict = td_kf_.getLatestP(); + Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); + x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; + x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); + x_ex_t = x_tmp; + Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); + P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; + P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = + A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); + P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); + P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = + P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); + P_ex_t = P_tmp; + Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); + EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); + EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); + EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); + EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); + + // Define update parameters + Eigen::MatrixXd C_t(3, 3); + C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; + Eigen::MatrixXd R_t(3, 3); + R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; + Eigen::MatrixXd y_t(3, 1); + y_t << 1.05, 2.05, 3.05; + const int delay_step = 2; // Choose an appropriate delay step + const int dim_y = y_t.rows(); + + // Perform update + EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); + + // Check the updated state and covariance matrix + Eigen::MatrixXd x_update = td_kf_.getLatestX(); + Eigen::MatrixXd P_update = td_kf_.getLatestP(); + + Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); + const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); + const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); + const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; + x_ex_t = x_ex_t + K_t * (y_t - y_pred); + P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); + Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); + Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); + EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); + EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); + EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); + EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); + EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); + EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); +} From 84995ec3ca4e201ed1d800c0eeb967de25298f84 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 11 Oct 2023 18:37:33 +0900 Subject: [PATCH 058/206] feat(intersection): not detect stuck vehicle when turning left (#5268) Signed-off-by: Takayuki Murooka --- .../src/scene_intersection.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index fb35834ab39a5..26bc8210b3bad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1213,6 +1213,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( bool IntersectionModule::checkStuckVehicle( const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { + // NOTE: No need to stop for stuck vehicle when the ego will turn left. + if (turn_direction_ == std::string("left")) { + return false; + } + const auto & objects_ptr = planner_data->predicted_objects; // considering lane change in the intersection, these lanelets are generated from the path From 6b58314939716775b7253c07f1140928af4e28ac Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 11:06:01 +0900 Subject: [PATCH 059/206] refactor(motion_velocity_smoother): align processing time topic name (#5273) Signed-off-by: satoshi-ota --- .../src/motion_velocity_smoother_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index 75c84d3c482f6..6b5da01b9cb09 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -79,7 +79,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions debug_closest_acc_ = create_publisher("~/closest_acceleration", 1); debug_closest_jerk_ = create_publisher("~/closest_jerk", 1); debug_closest_max_velocity_ = create_publisher("~/closest_max_velocity", 1); - debug_calculation_time_ = create_publisher("~/calculation_time", 1); + debug_calculation_time_ = create_publisher("~/debug/processing_time_ms", 1); pub_trajectory_raw_ = create_publisher("~/debug/trajectory_raw", 1); pub_trajectory_vel_lim_ = create_publisher("~/debug/trajectory_external_velocity_limited", 1); From 7e0ee1eba48862ec0a6579e3db578c128aa4d8cb Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 12 Oct 2023 11:49:11 +0900 Subject: [PATCH 060/206] feat(lane_change): keep distance against avoidable stationary objects (#5236) * feat(lane_change): keep distance against avoidable stationary objects Signed-off-by: kosuke55 consider rss distance Signed-off-by: kosuke55 tmp Signed-off-by: kosuke55 * fix dangling Signed-off-by: Takamasa Horibe * use parameter for velocity_treshold Signed-off-by: Takamasa Horibe * check object only on the ego path Signed-off-by: Takamasa Horibe * fix Signed-off-by: Takamasa Horibe --------- Signed-off-by: kosuke55 Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe --- .../src/scene_module/lane_change/normal.cpp | 83 ++++++++++++++++--- 1 file changed, 73 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 71ba8c51ec6ad..46812d9c487a9 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -37,6 +37,8 @@ namespace behavior_path_planner { +using motion_utils::calcSignedArcLength; + NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, Direction direction) @@ -139,7 +141,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() if (isStopState()) { const auto current_velocity = getEgoVelocity(); - const auto current_dist = motion_utils::calcSignedArcLength( + const auto current_dist = calcSignedArcLength( output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); @@ -202,7 +204,70 @@ void NormalLaneChange::insertStopPoint( } const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; - const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); + const bool is_in_terminal_section = lanelet::utils::isInLanelet(getEgoPose(), lanelets.back()) || + distance_to_terminal < lane_change_buffer; + const bool has_blocking_target_lane_obj = std::any_of( + target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + if (o.initial_twist.twist.linear.x > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + const double distance_to_target_lane_obj = utils::getSignedDistance( + path.points.front().point.pose, o.initial_pose.pose, status_.target_lanes); + return distance_to_target_lane_obj < distance_to_terminal; + }); + + // auto stopping_distance = raw_stopping_distance; + double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + if (is_in_terminal_section || !has_blocking_target_lane_obj) { + // calculate minimum distance from path front to the stationary object on the ego lane. + const auto distance_to_ego_lane_obj = [&]() -> double { + double distance_to_obj = distance_to_terminal; + const double distance_to_ego = + utils::getSignedDistance(path.points.front().point.pose, getEgoPose(), lanelets); + + for (const auto & object : target_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : polygon) { + const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + + // ignore if the point is around the ego path + if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { + continue; + } + + const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + + // ignore backward object + if (current_distance_to_obj < distance_to_ego) { + continue; + } + distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); + } + } + return distance_to_obj; + }(); + + // If the lane change space is occupied by a stationary obstacle, move the stop line closer. + // TODO(Horibe): We need to loop this process because the new space could be occupied as well. + stopping_distance = std::min( + distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer - + getCommonParam().base_link2front - + calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params), + stopping_distance); + } + if (stopping_distance > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); setStopPose(stop_point.point.pose); @@ -220,8 +285,7 @@ std::optional NormalLaneChange::extendPath() const auto path = status_.lane_change_path.path; const auto lc_start_point = status_.lane_change_path.info.lane_changing_start.position; - const auto dist = - motion_utils::calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); + const auto dist = calcSignedArcLength(path.points, lc_start_point, getEgoPosition()); if (dist < 0.0) { return std::nullopt; @@ -423,7 +487,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -439,7 +503,7 @@ bool NormalLaneChange::isEgoOnPreparePhase() const { const auto & start_position = status_.lane_change_path.info.shift_line.start.position; const auto & path_points = status_.lane_change_path.path.points; - return motion_utils::calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; + return calcSignedArcLength(path_points, start_position, getEgoPosition()) < 0.0; } bool NormalLaneChange::isAbleToStopSafely() const @@ -459,7 +523,7 @@ bool NormalLaneChange::isAbleToStopSafely() const double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { - dist += motion_utils::calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); + dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( @@ -475,7 +539,7 @@ bool NormalLaneChange::hasFinishedAbort() const return true; } - const auto distance_to_finish = motion_utils::calcSignedArcLength( + const auto distance_to_finish = calcSignedArcLength( abort_path_->path.points, getEgoPosition(), abort_path_->info.shift_line.end.position); if (distance_to_finish < 0.0) { @@ -720,8 +784,7 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( double min_dist_ego_to_obj = std::numeric_limits::max(); for (const auto & polygon_p : obj_polygon.outer()) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); - const double dist_ego_to_obj = - motion_utils::calcSignedArcLength(path.points, current_pose.position, obj_p); + const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } From e58eb3262d81a1f1bdf6730c0e42c5b8fad148f6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 12 Oct 2023 12:18:48 +0900 Subject: [PATCH 061/206] feat(lane_change): sampling all possible longitudinal acceleration when the ego is in stuck (#5249) * [lane_change] sampling all possible longitudinal acceleration when the ego is in stuck Signed-off-by: Takamasa Horibe * add clock Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Fumiya Watanabe * fix style Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Co-authored-by: Fumiya Watanabe --- .../scene_module/lane_change/normal.hpp | 13 ++ .../src/scene_module/lane_change/normal.cpp | 120 ++++++++++++++---- 2 files changed, 108 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index f1e4483c8bf51..4f1eb7340fdff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -153,6 +153,19 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. + //! @param obstacle_check_distance Distance to check ahead for any objects that might be + //! obstructing ego path. It makes sense to use values like the maximum lane change distance. + bool isVehicleStuckByObstacle( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; + + bool isVehicleStuckByObstacle(const lanelet::ConstLanelets & current_lanes) const; + + double calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; + + std::pair calcCurrentMinMaxAcceleration() const; + void setStopPose(const Pose & stop_pose); void updateStopTime(); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 46812d9c487a9..4075202367a65 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -572,6 +572,37 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane, get_opposite_direction)); } +std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() const +{ + const auto & p = getCommonParam(); + + const auto vehicle_min_acc = std::max(p.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto vehicle_max_acc = std::min(p.max_acc, lane_change_parameters_->max_longitudinal_acc); + + const auto ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_module_path_.points, getEgoPose(), p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold); + const auto max_path_velocity = + prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps; + + // calculate minimum and maximum acceleration + const auto min_acc = + utils::lane_change::calcMinimumAcceleration(getEgoVelocity(), vehicle_min_acc, p); + const auto max_acc = utils::lane_change::calcMaximumAcceleration( + getEgoVelocity(), max_path_velocity, vehicle_max_acc, p); + + return {min_acc, max_acc}; +} + +double NormalLaneChange::calcMaximumLaneChangeLength( + const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const +{ + const auto shift_intervals = + getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); + return utils::lane_change::calcMaximumLaneChangeLength( + getEgoVelocity(), getCommonParam(), shift_intervals, max_acc); +} + std::vector NormalLaneChange::sampleLongitudinalAccValues( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { @@ -579,29 +610,11 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return {}; } - const auto & common_parameters = planner_data_->parameters; const auto & route_handler = *getRouteHandler(); const auto current_pose = getEgoPose(); - const auto current_velocity = getEgoVelocity(); - const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; - const auto vehicle_min_acc = - std::max(common_parameters.min_acc, lane_change_parameters_->min_longitudinal_acc); - const auto vehicle_max_acc = - std::min(common_parameters.max_acc, lane_change_parameters_->max_longitudinal_acc); - const double nearest_dist_threshold = common_parameters.ego_nearest_dist_threshold; - const double nearest_yaw_threshold = common_parameters.ego_nearest_yaw_threshold; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_module_path_.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double & max_path_velocity = - prev_module_path_.points.at(current_seg_idx).point.longitudinal_velocity_mps; - - // calculate minimum and maximum acceleration - const auto min_acc = utils::lane_change::calcMinimumAcceleration( - current_velocity, vehicle_min_acc, common_parameters); - const auto max_acc = utils::lane_change::calcMaximumAcceleration( - current_velocity, max_path_velocity, vehicle_max_acc, common_parameters); + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { @@ -610,15 +623,22 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // calculate maximum lane change length - const double max_lane_change_length = utils::lane_change::calcMaximumLaneChangeLength( - current_velocity, common_parameters, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), max_acc); + const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } + // If the ego is in stuck, sampling all possible accelerations to find avoiding path. + if (isVehicleStuckByObstacle(current_lanes, max_lane_change_length)) { + auto clock = rclcpp::Clock(RCL_ROS_TIME); + RCLCPP_INFO_THROTTLE( + logger_, clock, 1000, "Vehicle is stuck. sample all longitudinal acceleration."); + return utils::lane_change::getAccelerationValues( + min_acc, max_acc, longitudinal_acc_sampling_num); + } + // if maximum lane change length is less than length to goal or the end of target lanes, only // sample max acc if (route_handler.isInGoalRouteSection(target_lanes.back())) { @@ -1185,8 +1205,9 @@ bool NormalLaneChange::getLaneChangePaths( if (getStopTime() < lane_change_parameters_->stop_time_threshold) { continue; } - RCLCPP_WARN_STREAM( - logger_, "Stop time is over threshold. Allow lane change in crosswalk."); + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_THROTTLE( + logger_, clock, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); } if ( @@ -1515,8 +1536,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); auto collision_check_objects = target_objects.target_lane; + const auto current_lanes = getCurrentLanes(); + const auto is_stuck = isVehicleStuckByObstacle(current_lanes); - if (lane_change_parameters_->check_objects_on_current_lanes) { + if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) { collision_check_objects.insert( collision_check_objects.end(), target_objects.current_lane.begin(), target_objects.current_lane.end()); @@ -1571,6 +1594,53 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return path_safety_status; } +// Check if the ego vehicle is in stuck by a stationary obstacle. +bool NormalLaneChange::isVehicleStuckByObstacle( + const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const +{ + // Ego is still moving, not in stuck + if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + + // Ego is just stopped, not sure it is in stuck yet. + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + return false; + } + + // Check if any stationary object exist in obstacle_check_distance + using lanelet::utils::getArcCoordinates; + const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; + + for (const auto & object : debug_filtered_objects_.current_lane) { + const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + + // Note: it needs chattering prevention. + if (std::abs(object.initial_twist.twist.linear.x) > 0.3) { // check if stationary + continue; + } + + const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; + if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { + return true; // Stationary object is in front of ego. + } + } + + // No stationary objects found in obstacle_check_distance + return false; +} + +bool NormalLaneChange::isVehicleStuckByObstacle(const lanelet::ConstLanelets & current_lanes) const +{ + if (current_lanes.empty()) { + return false; // can not check + } + + const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); + const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); + return isVehicleStuckByObstacle(current_lanes, max_lane_change_length); +} + void NormalLaneChange::setStopPose(const Pose & stop_pose) { lane_change_stop_pose_ = stop_pose; From 29279a0f736c7c6c02cccd2c782a5a1b792fb0b2 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 12:41:28 +0900 Subject: [PATCH 062/206] feat(planning_validator): output processing time (#5276) Signed-off-by: satoshi-ota --- .../planning_validator/planning_validator.hpp | 8 ++++++++ .../planning_validator/src/planning_validator.cpp | 12 ++++++++++++ 2 files changed, 20 insertions(+) diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 3b09ebe51ff6b..ef570b57773bb 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -18,6 +18,7 @@ #include "planning_validator/debug_marker.hpp" #include "planning_validator/msg/planning_validator_status.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -26,6 +27,7 @@ #include #include #include +#include #include #include @@ -38,6 +40,8 @@ using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; using planning_validator::msg::PlanningValidatorStatus; +using tier4_autoware_utils::StopWatch; +using tier4_debug_msgs::msg::Float64Stamped; struct ValidationParams { @@ -81,6 +85,7 @@ class PlanningValidator : public rclcpp::Node void validate(const Trajectory & trajectory); + void publishProcessingTime(const double processing_time_ms); void publishTrajectory(); void publishDebugInfo(); void displayStatus(); @@ -91,6 +96,7 @@ class PlanningValidator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; rclcpp::Publisher::SharedPtr pub_status_; + rclcpp::Publisher::SharedPtr pub_processing_time_ms_; rclcpp::Publisher::SharedPtr pub_markers_; // system parameters @@ -120,6 +126,8 @@ class PlanningValidator : public rclcpp::Node std::shared_ptr debug_pose_publisher_; std::unique_ptr logger_configure_; + + StopWatch stop_watch_; }; } // namespace planning_validator diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 13a63a7e7d4ed..875781d47b25d 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -41,6 +41,7 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) pub_traj_ = create_publisher("~/output/trajectory", 1); pub_status_ = create_publisher("~/output/validation_status", 1); pub_markers_ = create_publisher("~/output/markers", 1); + pub_processing_time_ms_ = create_publisher("~/debug/processing_time_ms", 1); debug_pose_publisher_ = std::make_shared(this); @@ -167,6 +168,8 @@ bool PlanningValidator::isDataReady() void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) { + stop_watch_.tic(__func__); + current_trajectory_ = msg; if (!isDataReady()) return; @@ -180,6 +183,7 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) publishTrajectory(); // for debug + publishProcessingTime(stop_watch_.toc(__func__)); publishDebugInfo(); displayStatus(); } @@ -222,6 +226,14 @@ void PlanningValidator::publishTrajectory() return; } +void PlanningValidator::publishProcessingTime(const double processing_time_ms) +{ + Float64Stamped msg{}; + msg.stamp = this->now(); + msg.data = processing_time_ms; + pub_processing_time_ms_->publish(msg); +} + void PlanningValidator::publishDebugInfo() { validation_status_.stamp = get_clock()->now(); From 79e64d832c979e0cceae48e4eab2d1459db85113 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 12:52:29 +0900 Subject: [PATCH 063/206] feat(obstacle_stop_planner): output processing time (#5279) Signed-off-by: satoshi-ota --- .../include/obstacle_stop_planner/node.hpp | 8 ++++++++ planning/obstacle_stop_planner/src/node.cpp | 9 +++++++++ 2 files changed, 17 insertions(+) diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 93a9736bcdb36..368ad34b9c4e2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -19,6 +19,7 @@ #include "obstacle_stop_planner/debug_marker.hpp" #include "obstacle_stop_planner/planner_data.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/system/stop_watch.hpp" #include #include @@ -37,6 +38,7 @@ #include #include #include +#include #include #include #include @@ -78,9 +80,11 @@ using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::BoolStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -143,6 +147,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_collision_pointcloud_debug_; + rclcpp::Publisher::SharedPtr pub_processing_time_ms_; + std::unique_ptr acc_controller_; std::shared_ptr debug_ptr_; boost::optional latest_slow_down_section_{boost::none}; @@ -166,6 +172,8 @@ class ObstacleStopPlannerNode : public rclcpp::Node StopParam stop_param_; SlowDownParam slow_down_param_; + StopWatch stop_watch_; + // mutex for vehicle_info_, stop_param_, current_acc_, obstacle_ros_pointcloud_ptr_ // NOTE: shared_ptr itself is thread safe so we do not have to care if *ptr is not used // (current_velocity_ptr_) diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 4e4b5d2f91ef3..9a9712c5a2503 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -209,6 +209,8 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod pub_collision_pointcloud_debug_ = this->create_publisher("~/debug/collision_pointcloud", 1); + pub_processing_time_ms_ = this->create_publisher("~/debug/processing_time_ms", 1); + // Subscribers if (!node_param_.use_predicted_objects) { // No need to point cloud while using predicted objects @@ -274,6 +276,8 @@ void ObstacleStopPlannerNode::onPointCloud(const PointCloud2::ConstSharedPtr inp void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_msg) { + stop_watch_.tic(__func__); + mutex_.lock(); // NOTE: these variables must not be referenced for multithreading const auto vehicle_info = vehicle_info_; @@ -376,6 +380,11 @@ void ObstacleStopPlannerNode::onTrigger(const Trajectory::ConstSharedPtr input_m trajectory.header = input_msg->header; pub_trajectory_->publish(trajectory); + + Float64Stamped processing_time_ms; + processing_time_ms.stamp = now(); + processing_time_ms.data = stop_watch_.toc(__func__); + pub_processing_time_ms_->publish(processing_time_ms); } void ObstacleStopPlannerNode::searchObstacle( From 10bb43cab859d90802ffbfe6114885726974fdb4 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 12:54:03 +0900 Subject: [PATCH 064/206] refactor(path_smoother, obstacle_avoidance_planner): output processing time topic (#5274) * refactor(path_smoother): output processing time topic Signed-off-by: satoshi-ota * refactor(obstacle_avoidance_planner): output processing time topic Signed-off-by: satoshi-ota * Update planning/path_smoother/src/elastic_band_smoother.cpp Co-authored-by: Takamasa Horibe * Update planning/path_smoother/src/elastic_band_smoother.cpp Co-authored-by: Takamasa Horibe * Update planning/obstacle_avoidance_planner/src/node.cpp Co-authored-by: Takamasa Horibe * Update planning/obstacle_avoidance_planner/src/node.cpp Co-authored-by: Takamasa Horibe --------- Signed-off-by: satoshi-ota Co-authored-by: Takamasa Horibe --- .../common_structs.hpp | 11 ++++++++++- .../include/obstacle_avoidance_planner/node.hpp | 3 ++- .../obstacle_avoidance_planner/type_alias.hpp | 2 ++ planning/obstacle_avoidance_planner/src/node.cpp | 16 ++++++++++++++-- .../include/path_smoother/common_structs.hpp | 11 ++++++++++- .../path_smoother/elastic_band_smoother.hpp | 3 ++- .../include/path_smoother/type_alias.hpp | 2 ++ .../path_smoother/src/elastic_band_smoother.cpp | 16 ++++++++++++++-- 8 files changed, 56 insertions(+), 8 deletions(-) diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp index d0fc995da0ef4..4f4f01bf1ec9f 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/common_structs.hpp @@ -45,7 +45,11 @@ struct PlannerData struct TimeKeeper { - void init() { accumulated_msg = "\n"; } + void init() + { + accumulated_msg = "\n"; + accumulated_time = 0.0; + } template TimeKeeper & operator<<(const T & msg) @@ -71,6 +75,7 @@ struct TimeKeeper { const double elapsed_time = stop_watch_.toc(func_name); *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; + accumulated_time = elapsed_time; endLine(); } @@ -80,6 +85,10 @@ struct TimeKeeper std::string accumulated_msg = "\n"; std::stringstream latest_stream; + double getAccumulatedTime() const { return accumulated_time; } + + double accumulated_time{0.0}; + tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index d6f49afad4391..9207277d0bc98 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -90,7 +90,8 @@ class ObstacleAvoidancePlanner : public rclcpp::Node // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; rclcpp::Publisher::SharedPtr debug_markers_pub_; - rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp index 0f45e1223551f..b02d2232a4aa1 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/type_alias.hpp @@ -24,6 +24,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -42,6 +43,7 @@ using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug +using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; } // namespace obstacle_avoidance_planner diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index 9cee933a1c9a7..da31c7f469555 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -45,6 +45,14 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } +Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} + void setZeroVelocityAfterStopPoint(std::vector & traj_points) { const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); @@ -92,7 +100,9 @@ ObstacleAvoidancePlanner::ObstacleAvoidancePlanner(const rclcpp::NodeOptions & n // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); debug_markers_pub_ = create_publisher("~/debug/marker", 1); - debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_float_pub_ = + create_publisher("~/debug/processing_time_ms", 1); { // parameters // parameter for option @@ -253,7 +263,9 @@ void ObstacleAvoidancePlanner::onPath(const Path::ConstSharedPtr path_ptr) // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); - debug_calculation_time_pub_->publish(calculation_time_msg); + debug_calculation_time_str_pub_->publish(calculation_time_msg); + debug_calculation_time_float_pub_->publish( + createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/path_smoother/include/path_smoother/common_structs.hpp index a14c42ed056af..d44c964cf634c 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/path_smoother/include/path_smoother/common_structs.hpp @@ -44,7 +44,11 @@ struct PlannerData struct TimeKeeper { - void init() { accumulated_msg = "\n"; } + void init() + { + accumulated_msg = "\n"; + accumulated_time = 0.0; + } template TimeKeeper & operator<<(const T & msg) @@ -66,6 +70,7 @@ struct TimeKeeper { const double elapsed_time = stop_watch_.toc(func_name); *this << white_spaces << func_name << ":= " << elapsed_time << " [ms]"; + accumulated_time = elapsed_time; endLine(); } @@ -74,6 +79,10 @@ struct TimeKeeper std::string accumulated_msg = "\n"; std::stringstream latest_stream; + double getAccumulatedTime() const { return accumulated_time; } + + double accumulated_time{0.0}; + tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index a67983393681f..217a138084310 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -82,7 +82,8 @@ class ElasticBandSmoother : public rclcpp::Node // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; - rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_str_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_float_pub_; // parameter callback rcl_interfaces::msg::SetParametersResult onParam( diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index 75bf1cff20857..c8f6d6da98dcd 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -22,6 +22,7 @@ #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" namespace path_smoother @@ -36,6 +37,7 @@ using autoware_auto_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // debug +using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; } // namespace path_smoother diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index cd314b7b141bf..923b753e83ac6 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -43,6 +43,14 @@ StringStamped createStringStamped(const rclcpp::Time & now, const std::string & return msg; } +Float64Stamped createFloat64Stamped(const rclcpp::Time & now, const float & data) +{ + Float64Stamped msg; + msg.stamp = now; + msg.data = data; + return msg; +} + void setZeroVelocityAfterStopPoint(std::vector & traj_points) { const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points); @@ -75,7 +83,9 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); - debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_str_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_float_pub_ = + create_publisher("~/debug/processing_time_ms", 1); { // parameters // parameters for ego nearest search @@ -205,7 +215,9 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) // publish calculation_time // NOTE: This function must be called after measuring onPath calculation time const auto calculation_time_msg = createStringStamped(now(), time_keeper_ptr_->getLog()); - debug_calculation_time_pub_->publish(calculation_time_msg); + debug_calculation_time_str_pub_->publish(calculation_time_msg); + debug_calculation_time_float_pub_->publish( + createFloat64Stamped(now(), time_keeper_ptr_->getAccumulatedTime())); const auto output_traj_msg = trajectory_utils::createTrajectory(path_ptr->header, full_traj_points); From 021d54057b4d9163248885e06ecd8a89a4f943cb Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 13:08:38 +0900 Subject: [PATCH 065/206] fix(avoidance): align execution request condition (#5266) Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index a8dd0b794245e..632aad8873906 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -171,7 +171,10 @@ bool AvoidanceModule::isExecutionRequested() const return false; } - return !avoid_data_.target_objects.empty(); + return std::any_of( + avoid_data_.target_objects.begin(), avoid_data_.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); } bool AvoidanceModule::isExecutionReady() const From bc1d6c74afe7ac2af57d43b10320a1635113bb89 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 13:19:39 +0900 Subject: [PATCH 066/206] refactor(behavior_path_planner): align processing time topic name (#5281) Signed-off-by: satoshi-ota --- planning/behavior_path_planner/src/planner_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 24967608775f3..d0e55ffa574fd 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -35,7 +35,7 @@ PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) verbose_{verbose} { processing_time_.emplace("total_time", 0.0); - debug_publisher_ptr_ = std::make_unique(&node, "behavior_planner_manager/debug"); + debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); } BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & data) @@ -894,7 +894,7 @@ void PlannerManager::print() const void PlannerManager::publishProcessingTime() const { for (const auto & t : processing_time_) { - std::string name = std::string("processing_time/") + t.first; + std::string name = t.first + std::string("/processing_time_ms"); debug_publisher_ptr_->publish(name, t.second); } } From a220a96169e8c6b7b214493451f10447070a59dc Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 14:01:36 +0900 Subject: [PATCH 067/206] fix(avoidance): expand detection area to prevent chattering (#5267) Signed-off-by: satoshi-ota --- .../utils/avoidance/utils.hpp | 2 +- .../avoidance/avoidance_module.cpp | 6 ++++- .../src/utils/avoidance/utils.cpp | 24 ++++++++++++++++++- 3 files changed, 29 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index c0bd621cc86b4..d012fd5f612fb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -164,7 +164,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug); + const bool is_running, DebugData & debug); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 632aad8873906..8edc291fd560d 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -292,12 +292,16 @@ void AvoidanceModule::fillAvoidanceTargetObjects( using utils::avoidance::filterTargetObjects; using utils::avoidance::getTargetLanelets; + // Add margin in order to prevent avoidance request chattering only when the module is running. + const auto is_running = getCurrentStatus() == ModuleStatus::RUNNING || + getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; + // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. const auto [object_within_target_lane, object_outside_target_lane] = utils::avoidance::separateObjectsByPath( utils::resamplePathWithSpline( helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output), - planner_data_, data, parameters_, debug); + planner_data_, data, parameters_, is_running, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 0e7eb87feed9b..ed2e42bf0c5ff 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -29,9 +29,12 @@ #include +#include #include #include #include +#include +#include #include #include @@ -1602,7 +1605,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - DebugData & debug) + const bool is_running, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1639,6 +1642,25 @@ std::pair separateObjectsByPath( } } + // expand detection area width only when the module is running. + if (is_running) { + constexpr int PER_CIRCLE = 36; + constexpr double MARGIN = 1.0; // [m] + boost::geometry::strategy::buffer::distance_symmetric distance_strategy(MARGIN); + boost::geometry::strategy::buffer::join_round join_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::end_round end_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::point_circle circle_strategy(PER_CIRCLE); + boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::model::multi_polygon result; + // Create the buffer of a multi polygon + boost::geometry::buffer( + attention_area, result, distance_strategy, side_strategy, join_strategy, end_strategy, + circle_strategy); + if (!result.empty()) { + attention_area = result.front(); + } + } + debug.detection_area = toMsg(attention_area, data.reference_pose.position.z); const auto objects = planner_data->dynamic_object->objects; From bc60bbfb659be580f6e8aa894d2f16b9afbb4c0d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 14:03:20 +0900 Subject: [PATCH 068/206] refactor(obstacle_cruise_planner): align processing time topic name (#5278) Signed-off-by: satoshi-ota --- planning/obstacle_cruise_planner/src/node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index d50054da46237..c9fe9f2210423 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -374,7 +374,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); // debug publisher - debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); + debug_calculation_time_pub_ = create_publisher("~/debug/processing_time_ms", 1); debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); debug_slow_down_wall_marker_pub_ = From f872838baeeb43e4d8bf10140676ce75151d09a1 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 12 Oct 2023 14:31:22 +0900 Subject: [PATCH 069/206] chore(intersection): parameterize stuck vehicle detection turn_direction (#5277) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 4 ++++ .../behavior_velocity_intersection_module/src/manager.cpp | 6 ++++++ .../src/scene_intersection.cpp | 8 ++++++-- .../src/scene_intersection.hpp | 6 ++++++ 4 files changed, 22 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 30fefcaeee035..bb80c140b95fb 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -13,6 +13,10 @@ path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false stuck_vehicle: + turn_direction: + left: true + right: true + straight: true use_stuck_stopline: true # stopline generated before the first conflicting area stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 4137090a5b6ae..3cca1f42115d1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -63,6 +63,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.consider_wrong_direction_vehicle = getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); + ip.stuck_vehicle.turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); + ip.stuck_vehicle.turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.right"); + ip.stuck_vehicle.turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.straight"); ip.stuck_vehicle.use_stuck_stopline = getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 26bc8210b3bad..ee541a061ce0e 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1213,8 +1213,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( bool IntersectionModule::checkStuckVehicle( const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets) { - // NOTE: No need to stop for stuck vehicle when the ego will turn left. - if (turn_direction_ == std::string("left")) { + const bool stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && planner_param_.stuck_vehicle.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.stuck_vehicle.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.stuck_vehicle.turn_direction.straight); + }(); + if (!stuck_detection_direction) { return false; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index de7d97a50c133..6ae734bd6e05b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -61,6 +61,12 @@ class IntersectionModule : public SceneModuleInterface } common; struct StuckVehicle { + struct TurnDirection + { + bool left; + bool right; + bool straight; + } turn_direction; bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped From 02eb8434ed634b93c334bab26825fd97cbc94ab3 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 12 Oct 2023 17:22:10 +0900 Subject: [PATCH 070/206] fix(lane_change): fixed debug visualization bug (#5284) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 4075202367a65..69c39b41efb06 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1560,12 +1560,14 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto current_debug_data = marker_utils::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); + auto is_safe = true; for (const auto & obj_path : obj_predicted_paths) { const auto collided_polygons = utils::path_safety_checker::getCollidedPolygons( path, ego_predicted_path, obj, obj_path, common_parameters, rss_params, 1.0, current_debug_data.second); if (collided_polygons.empty()) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } @@ -1575,20 +1577,20 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); continue; } + is_safe = false; path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); } - marker_utils::updateCollisionCheckDebugMap( - debug_data, current_debug_data, path_safety_status.is_safe); + marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); } return path_safety_status; From 75debf01597726df68399519777fd0f78f15dc84 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 12 Oct 2023 18:02:48 +0900 Subject: [PATCH 071/206] feat(avoidance): check if the avoidance path is in drivable area (#5032) * feat(avoidance): check if the avoidance path is in drivable area Signed-off-by: satoshi-ota * refactor(avoidance): remove redundant function Signed-off-by: satoshi-ota * fix(utils): add guard Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 1 + .../avoidance/avoidance_module.hpp | 10 +- .../utils/avoidance/avoidance_module_data.hpp | 11 ++ .../avoidance/avoidance_module.cpp | 142 ++++++++++++------ .../src/scene_module/avoidance/manager.cpp | 2 + .../behavior_path_planner/src/utils/utils.cpp | 9 +- 6 files changed, 120 insertions(+), 55 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index df2dd806fe2c7..2b38536341d7c 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -169,6 +169,7 @@ hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 + max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index d362a5e758a3e..cbf40e59535be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -334,11 +334,10 @@ class AvoidanceModule : public SceneModuleInterface /* * @brief generate candidate shift lines. * @param one-shot shift lines. - * @param path shifter. * @param debug data. */ AvoidLineArray generateCandidateShiftLine( - const AvoidLineArray & shift_lines, const PathShifter & path_shifter, DebugData & debug) const; + const AvoidLineArray & shift_lines, DebugData & debug) const; /** * @brief clean up raw shift lines. @@ -478,13 +477,6 @@ class AvoidanceModule : public SceneModuleInterface */ TurnSignalInfo calcTurnSignalInfo(const ShiftedPath & path) const; - // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep - // extending during avoidance module - // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes - // NOTE: Assume that there is no situation where there is an object in the middle lane of more - // than two lanes since which way to avoid is not obvious - void generateExpandDrivableLanes(BehaviorModuleOutput & output) const; - /** * @brief fill debug markers. */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 6173ecb15a5c2..5ba0085ded5b7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -232,6 +232,9 @@ struct AvoidanceParameters // Even if the obstacle is very large, it will not avoid more than this length for left direction double max_left_shift_length{0.0}; + // Validate vehicle departure from driving lane. + double max_deviation_from_lane{0.0}; + // To prevent large acceleration while avoidance. double max_lateral_acceleration{0.0}; @@ -490,8 +493,16 @@ struct AvoidancePlanningData // safe shift point AvoidLineArray safe_shift_line{}; + std::vector drivable_lanes{}; + + lanelet::BasicLineString3d right_bound{}; + + lanelet::BasicLineString3d left_bound{}; + bool safe{false}; + bool valid{false}; + bool comfortable{false}; bool avoid_required{false}; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 8edc291fd560d..c94406e466219 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -143,6 +143,14 @@ AvoidLineArray toArray(const AvoidOutlines & outlines) } return ret; } + +lanelet::BasicLineString3d toLineString3d(const std::vector & bound) +{ + lanelet::BasicLineString3d ret{}; + std::for_each( + bound.begin(), bound.end(), [&](const auto & p) { ret.emplace_back(p.x, p.y, p.z); }); + return ret; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -248,6 +256,23 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( *getPreviousModuleOutput().reference_path, planner_data_); + // expand drivable lanes + std::for_each( + data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { + data.drivable_lanes.push_back( + utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); + }); + + // calc drivable bound + const auto shorten_lanes = + utils::cutOverlappedLanes(*getPreviousModuleOutput().path, data.drivable_lanes); + data.left_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, true)); + data.right_bound = toLineString3d(utils::calcBound( + planner_data_->route_handler, shorten_lanes, parameters_->use_hatched_road_markings, + parameters_->use_intersection_areas, false)); + // reference path if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); @@ -447,13 +472,21 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de * STEP3: Create candidate shift lines. * Merge rough shift lines, and extract new shift lines. */ - data.new_shift_line = generateCandidateShiftLine(data.raw_shift_line, path_shifter, debug); + const auto processed_shift_lines = generateCandidateShiftLine(data.raw_shift_line, debug); + + /** + * Step4: Validate new shift lines. + * Output new shift lines only when the avoidance path which is generated from them doesn't have + * huge offset from ego. + */ + data.valid = isValidShiftLine(processed_shift_lines, path_shifter); + data.new_shift_line = data.valid ? processed_shift_lines : AvoidLineArray{}; const auto found_new_sl = data.new_shift_line.size() > 0; const auto registered = path_shifter.getShiftLines().size() > 0; data.found_avoidance_path = found_new_sl || registered; /** - * STEP4: Set new shift lines. + * STEP5: Set new shift lines. * If there are new shift points, these shift points are registered in path_shifter in order to * generate candidate avoidance path. */ @@ -462,7 +495,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de } /** - * STEP5: Generate avoidance path. + * STEP6: Generate avoidance path. */ ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = @@ -472,7 +505,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de : utils::avoidance::toShiftedPath(data.reference_path); /** - * STEP6: Check avoidance path safety. + * STEP7: Check avoidance path safety. * For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ @@ -483,6 +516,17 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de void AvoidanceModule::fillEgoStatus( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { + /** + * TODO(someone): prevent meaningless stop point insertion in other way. + * If the candidate shift line is invalid, manage all objects as unavoidable. + */ + if (!data.valid) { + std::for_each(data.target_objects.begin(), data.target_objects.end(), [](auto & o) { + o.is_avoidable = false; + o.reason = "InvalidShiftLine"; + }); + } + /** * Find the nearest object that should be avoid. When the ego follows reference path, * if the both of following two conditions are satisfied, the module surely avoid the object. @@ -769,7 +813,7 @@ AvoidLineArray AvoidanceModule::applyPreProcess( } AvoidLineArray AvoidanceModule::generateCandidateShiftLine( - const AvoidLineArray & shift_lines, const PathShifter & path_shifter, DebugData & debug) const + const AvoidLineArray & shift_lines, DebugData & debug) const { AvoidLineArray processed_shift_lines = shift_lines; @@ -789,15 +833,7 @@ AvoidLineArray AvoidanceModule::generateCandidateShiftLine( * Step3: Extract new shift lines. * Compare processed shift lines and registered shift lines in order to find new shift lines. */ - processed_shift_lines = findNewShiftLine(processed_shift_lines, debug); - - /** - * Step4: Validate new shift lines. - * Output new shift lines only when the avoidance path which is generated from them doesn't have - * huge offset from ego. - */ - return isValidShiftLine(processed_shift_lines, path_shifter) ? processed_shift_lines - : AvoidLineArray{}; + return findNewShiftLine(processed_shift_lines, debug); } void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) @@ -1921,34 +1957,6 @@ bool AvoidanceModule::isSafePath( return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } -void AvoidanceModule::generateExpandDrivableLanes(BehaviorModuleOutput & output) const -{ - std::vector drivable_lanes; - for (const auto & lanelet : avoid_data_.current_lanelets) { - drivable_lanes.push_back( - utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); - } - - { // for new architecture - DrivableAreaInfo current_drivable_area_info; - // generate drivable lanes - current_drivable_area_info.drivable_lanes = drivable_lanes; - // generate obstacle polygons - current_drivable_area_info.obstacles = - utils::avoidance::generateObstaclePolygonsForDrivableArea( - avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); - // expand hatched road markings - current_drivable_area_info.enable_expanding_hatched_road_markings = - parameters_->use_hatched_road_markings; - // expand intersection areas - current_drivable_area_info.enable_expanding_intersection_areas = - parameters_->use_intersection_areas; - - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); - } -} - PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const { const auto previous_path = helper_.getPreviousReferencePath(); @@ -2082,8 +2090,26 @@ BehaviorModuleOutput AvoidanceModule::plan() utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); // Drivable area generation. - generateExpandDrivableLanes(output); - setDrivableLanes(output.drivable_area_info.drivable_lanes); + { + DrivableAreaInfo current_drivable_area_info; + // generate drivable lanes + current_drivable_area_info.drivable_lanes = avoid_data_.drivable_lanes; + // generate obstacle polygons + current_drivable_area_info.obstacles = + utils::avoidance::generateObstaclePolygonsForDrivableArea( + avoid_data_.target_objects, parameters_, planner_data_->parameters.vehicle_width / 2.0); + // expand hatched road markings + current_drivable_area_info.enable_expanding_hatched_road_markings = + parameters_->use_hatched_road_markings; + // expand intersection areas + current_drivable_area_info.enable_expanding_intersection_areas = + parameters_->use_intersection_areas; + + output.drivable_area_info = utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + + setDrivableLanes(output.drivable_area_info.drivable_lanes); + } return output; } @@ -2338,7 +2364,7 @@ bool AvoidanceModule::isValidShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const { if (shift_lines.empty()) { - return false; + return true; } auto shifter_for_validate = shifter; @@ -2364,6 +2390,32 @@ bool AvoidanceModule::isValidShiftLine( } } + // check if the vehicle is in road. (yaw angle is not considered) + { + const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + + parameters_->hard_road_shoulder_margin - + parameters_->max_deviation_from_lane; + + const size_t start_idx = shift_lines.front().start_idx; + const size_t end_idx = shift_lines.back().end_idx; + + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(shifter_for_validate.getReferencePath().points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto bound = shift_length > 0.0 ? avoid_data_.left_bound : avoid_data_.right_bound; + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if (boost::geometry::distance(basic_point, lanelet::utils::to2D(bound)) < THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } + } + } + return true; // valid shift line. } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 8c65323e5a123..500ebe873a2d8 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -201,6 +201,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "lateral_avoid_check_threshold"); p.max_right_shift_length = getOrDeclareParameter(*node, ns + "max_right_shift_length"); p.max_left_shift_length = getOrDeclareParameter(*node, ns + "max_left_shift_length"); + p.max_deviation_from_lane = + getOrDeclareParameter(*node, ns + "max_deviation_from_lane"); } // avoidance maneuver (longitudinal) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 03063b5a9e2fe..bf6fffda41863 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1731,7 +1731,7 @@ std::vector getBoundWithIntersectionAreas( const auto shared_point_itr_last = std::find_if(expanded_bound.rbegin(), expanded_bound.rend(), [&](const auto & p) { return std::any_of( - intersection_bound.begin(), intersection_bound.end(), + intersection_bound.rbegin(), intersection_bound.rend(), [&](const auto & point) { return point.id() == p.id(); }); }); @@ -1757,6 +1757,13 @@ std::vector getBoundWithIntersectionAreas( continue; } + // TODO(Satoshi OTA): remove this guard. + if ( + std::distance(intersection_bound.begin(), trim_point_itr_last) < + std::distance(intersection_bound.begin(), trim_point_itr_init)) { + continue; + } + std::vector tmp_bound{}; tmp_bound.insert(tmp_bound.end(), expanded_bound.begin(), shared_point_itr_init); From 3cf0481b9b796d1a2dd325601467f5f98cd509ab Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 12 Oct 2023 18:35:24 +0900 Subject: [PATCH 072/206] fix(intersection): fix unsigned subtraction (#5280) Signed-off-by: Mamoru Sobue --- planning/behavior_velocity_intersection_module/src/util.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 7862a533f34eb..ffc0b61880188 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -475,6 +475,9 @@ std::optional getFirstPointInsidePolygon( if (is_in_lanelet) { return std::make_optional(i); } + if (i == 0) { + break; + } } } return std::nullopt; @@ -517,6 +520,9 @@ getFirstPointInsidePolygons( if (is_in_lanelet) { break; } + if (i == 0) { + break; + } } } return std::nullopt; From d5709ab4ffc99478042913a411ee57a4b50e0aa7 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 12 Oct 2023 19:09:30 +0900 Subject: [PATCH 073/206] fix(lane_change): update target lane if valid path not found (#5287) Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 69c39b41efb06..913ddcacf08bc 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -89,6 +89,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) if (valid_paths.empty()) { safe_path.info.current_lanes = current_lanes; + safe_path.info.target_lanes = target_lanes; return {false, false}; } From 4ba8923dd064b21749814f14bd30a5a97bd81e2e Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Thu, 12 Oct 2023 20:30:18 +0800 Subject: [PATCH 074/206] feat(tier4_system_rviz_plugin): improve visualization results and logics (#5222) * Add Init Localization and Init Planning Check; Add error list check Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * int casting problem updated Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * improve if statement writing styles Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * FIX ci Signed-off-by: Owen-Liuyuxuan * subscribe to ADAPI for more stable status checking Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * try using level attribute only Signed-off-by: Owen-Liuyuxuan * fix hpp so that it just look like the original hpp Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * set default value to NO_FAULT Signed-off-by: Owen-Liuyuxuan --------- Signed-off-by: Owen-Liuyuxuan Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- common/tier4_system_rviz_plugin/README.md | 10 +++++ .../src/mrm_summary_overlay_display.cpp | 40 ++++++++++++++----- 2 files changed, 40 insertions(+), 10 deletions(-) diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md index 098844c8b4091..61260ecfda723 100644 --- a/common/tier4_system_rviz_plugin/README.md +++ b/common/tier4_system_rviz_plugin/README.md @@ -1 +1,11 @@ # tier4_system_rviz_plugin + +## Purpose + +This plugin display the Hazard information from Autoware; and output notices when emergencies are from initial localization and route setting. + +## Input + +| Name | Type | Description | +| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index b80f06f645632..61c2bd378dab1 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -50,6 +50,7 @@ #include #include +#include #include #include @@ -126,6 +127,7 @@ MrmSummaryOverlayDisplay::~MrmSummaryOverlayDisplay() void MrmSummaryOverlayDisplay::onInitialize() { RTDClass::onInitialize(); + static int count = 0; rviz_common::UniformStringStream ss; ss << "MrmSummaryOverlayDisplayObject" << count++; @@ -160,9 +162,11 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) // MRM summary std::vector mrm_comfortable_stop_summary_list = {}; std::vector mrm_emergency_stop_summary_list = {}; + int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; { std::lock_guard message_lock(mutex_); if (last_msg_ptr_) { + hazard_level = last_msg_ptr_->status.level; for (const auto & diag_status : last_msg_ptr_->status.diag_latent_fault) { const std::optional msg = generateMrmMessage(diag_status); if (msg != std::nullopt) { @@ -201,16 +205,32 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) painter.setFont(font); std::ostringstream output_text; - output_text << std::fixed - << "Comfortable Stop MRM Summary: " << int(mrm_comfortable_stop_summary_list.size()) - << std::endl; - for (const auto & mrm_element : mrm_comfortable_stop_summary_list) { - output_text << mrm_element << std::endl; - } - output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size()) - << std::endl; - for (const auto & mrm_element : mrm_emergency_stop_summary_list) { - output_text << mrm_element << std::endl; + + // Display the MRM Summary only when there is a fault + if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) { + // Broadcasting the Basic Error Infos + int number_of_comfortable_stop_messages = + static_cast(mrm_comfortable_stop_summary_list.size()); + if (number_of_comfortable_stop_messages > 0) // Only Display when there are errors + { + output_text << std::fixed + << "Comfortable Stop MRM Summary: " << number_of_comfortable_stop_messages + << std::endl; + for (const auto & mrm_element : mrm_comfortable_stop_summary_list) { + output_text << mrm_element << std::endl; + } + } + + int number_of_emergency_stop_messages = + static_cast(mrm_emergency_stop_summary_list.size()); + if (number_of_emergency_stop_messages > 0) // Only Display when there are some errors + { + output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size()) + << std::endl; + for (const auto & mrm_element : mrm_emergency_stop_summary_list) { + output_text << mrm_element << std::endl; + } + } } // same as above, but align on right side From 744d48d0e3aaf9693062b44eb4c1edabc9659f6b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 12 Oct 2023 23:05:31 +0900 Subject: [PATCH 075/206] refactor(behavior_path_planner): remove planner data unused variable (#5194) refactor(behavior_path_planner): remove planner data used variable Signed-off-by: Zulfaqar Azmi --- .../include/behavior_path_planner/data_manager.hpp | 2 -- .../behavior_path_planner/test/test_drivable_area_expansion.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 0a7b59d52ff5a..7f1648c463097 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -141,11 +141,9 @@ struct PlannerData OccupancyGrid::ConstSharedPtr costmap{}; LateralOffset::ConstSharedPtr lateral_offset{}; OperationModeState::ConstSharedPtr operation_mode{}; - PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; std::optional prev_modified_goal{}; std::optional prev_route_id{}; - lanelet::ConstLanelets current_lanes{}; std::shared_ptr route_handler{std::make_shared()}; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index e0a16089b8d84..163221951d726 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -270,7 +270,6 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) } behavior_path_planner::PlannerData planner_data; planner_data.drivable_area_expansion_parameters = params; - planner_data.reference_path = std::make_shared(path); planner_data.dynamic_object = std::make_shared(dynamic_objects); planner_data.route_handler = std::make_shared(route_handler); From d8f285bf81ce33f55b604745cbdf91f8cc005af6 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 13 Oct 2023 04:52:39 +0900 Subject: [PATCH 076/206] fix(goal_planner): disable freespace pull over after arriving modified goal (#5290) fix(goal_planner): disable freespace pull over after arriving modified goal Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 9c2544cf60cec..c6e68f8e81af7 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -218,6 +218,10 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } + if (isOnModifiedGoal()) { + return; + } + const bool is_new_costmap = (clock_->now() - planner_data_->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; @@ -1210,6 +1214,10 @@ bool GoalPlannerModule::isStopped() bool GoalPlannerModule::isStuck() { + if (isOnModifiedGoal()) { + return false; + } + constexpr double stuck_time = 5.0; if (!isStopped(odometry_buffer_stuck_, stuck_time)) { return false; From e56698836e9c6491dd904c5178946639d37c89aa Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Fri, 13 Oct 2023 10:22:06 +0900 Subject: [PATCH 077/206] feat(tracking_object_merger): merge tracked object. especially for far radar object and conventional lidar object (#4340) * init package: migrate from object merger Signed-off-by: yoshiri * add node outline and check build passed Signed-off-by: yoshiri * add util functions to interpolate tracked objs Signed-off-by: yoshiri * add object merger function using interpolation Signed-off-by: yoshiri * create object merger utils Signed-off-by: yoshiri * add kinematics velocity merger Signed-off-by: yoshiri * add association and merger Signed-off-by: yoshiri * rename perception_utils to object_recognition_utils Signed-off-by: yoshiri * add comment and complete main subscriber Signed-off-by: yoshiri * add parameter control and rename some executable files Signed-off-by: yoshiri * refactoring: fix apparent bugs Signed-off-by: yoshiri * add debug tools to check association Signed-off-by: yoshiri * temporary fix: radar tracker node name to anon Signed-off-by: yoshiri * debug: data association tuning Signed-off-by: yoshiri * rename copyright and add merger util function Signed-off-by: yoshiri * add tracker_state and update association Signed-off-by: yoshiri * update decorative tracker by using tracker_state Signed-off-by: yoshiri * update system around measurement state function Signed-off-by: yoshiri * fix radar object not merged problem Signed-off-by: yoshiri * add existence probability control Signed-off-by: yoshiri * create const function Signed-off-by: yoshiri * change association settings depend on measurement and tracker state Signed-off-by: yoshiri * fix association matrix Signed-off-by: yoshiri * put hardcoded node parameter to yaml file Signed-off-by: yoshiri * move tracker state parameter to yaml file Signed-off-by: yoshiri * remove prediction failed objects Signed-off-by: yoshiri * fix bug when none closest time sub objects found Signed-off-by: yoshiri * add velocity diff gate in association Signed-off-by: yoshiri * fix object interpolation problem Signed-off-by: yoshiri * use fixed object interpolation Signed-off-by: yoshiri * add README Signed-off-by: yoshiri * add interpolated sub object publisher for debug Signed-off-by: yoshiri * add debug message and fix interpolation Signed-off-by: yoshiri * update README Signed-off-by: yoshiri * fix unintended changes in radar tracking launch Signed-off-by: yoshiri * add CmakeLists version Signed-off-by: yoshiri * remove unnecessary debug description and type cast causes build warning Signed-off-by: yoshiri * fix spell-check error Signed-off-by: yoshiri * replace autoware_utils.hpp Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../tracking_object_merger/CMakeLists.txt | 50 +++ perception/tracking_object_merger/README.md | 114 +++++ .../config/data_association_matrix.param.yaml | 168 ++++++++ .../decorative_tracker_merger.param.yaml | 26 ++ ...ecorative_tracker_merger_policy.param.yaml | 16 + .../decorative_tracker_merger.drawio.svg | 345 +++++++++++++++ .../image/time_sync.drawio.svg | 223 ++++++++++ .../image/tracklet_management.drawio.svg | 216 ++++++++++ .../data_association/data_association.hpp | 73 ++++ .../data_association/solver/gnn_solver.hpp | 22 + .../solver/gnn_solver_interface.hpp | 35 ++ .../solver/mu_successive_shortest_path.hpp | 37 ++ .../solver/successive_shortest_path.hpp | 37 ++ .../decorative_tracker_merger.hpp | 134 ++++++ .../utils/tracker_state.hpp | 148 +++++++ .../tracking_object_merger/utils/utils.hpp | 129 ++++++ .../decorative_tracker_merger.launch.xml | 21 + perception/tracking_object_merger/package.xml | 32 ++ .../src/data_association/data_association.cpp | 238 +++++++++++ .../mu_successive_shortest_path_wrapper.cpp | 41 ++ .../successive_shortest_path.cpp | 370 ++++++++++++++++ .../src/decorative_tracker_merger.cpp | 403 ++++++++++++++++++ .../src/utils/tracker_state.cpp | 321 ++++++++++++++ .../src/utils/utils.cpp | 403 ++++++++++++++++++ 24 files changed, 3602 insertions(+) create mode 100644 perception/tracking_object_merger/CMakeLists.txt create mode 100644 perception/tracking_object_merger/README.md create mode 100644 perception/tracking_object_merger/config/data_association_matrix.param.yaml create mode 100644 perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml create mode 100644 perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml create mode 100644 perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg create mode 100644 perception/tracking_object_merger/image/time_sync.drawio.svg create mode 100644 perception/tracking_object_merger/image/tracklet_management.drawio.svg create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp create mode 100644 perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp create mode 100644 perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml create mode 100644 perception/tracking_object_merger/package.xml create mode 100644 perception/tracking_object_merger/src/data_association/data_association.cpp create mode 100644 perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp create mode 100644 perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp create mode 100644 perception/tracking_object_merger/src/decorative_tracker_merger.cpp create mode 100644 perception/tracking_object_merger/src/utils/tracker_state.cpp create mode 100644 perception/tracking_object_merger/src/utils/utils.cpp diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt new file mode 100644 index 0000000000000..108749c5f91a7 --- /dev/null +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(tracking_object_merger VERSION 1.0.0) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror -Wconversion) +endif() + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(nlohmann_json REQUIRED) # for debug + + +# find dependencies +find_package(eigen3_cmake_module REQUIRED) +find_package(Eigen3 REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +include_directories( + SYSTEM + ${EIGEN3_INCLUDE_DIR} +) + +ament_auto_add_library(mu_successive_shortest_path SHARED + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp +) + +ament_auto_add_library(decorative_tracker_merger_node SHARED + src/data_association/data_association.cpp + src/decorative_tracker_merger.cpp + src/utils/utils.cpp + src/utils/tracker_state.cpp +) + +target_link_libraries(decorative_tracker_merger_node + mu_successive_shortest_path + Eigen3::Eigen + nlohmann_json::nlohmann_json # for debug +) + +rclcpp_components_register_node(decorative_tracker_merger_node + PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode" + EXECUTABLE decorative_tracker_merger +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/perception/tracking_object_merger/README.md b/perception/tracking_object_merger/README.md new file mode 100644 index 0000000000000..9964534372037 --- /dev/null +++ b/perception/tracking_object_merger/README.md @@ -0,0 +1,114 @@ +# Tracking Object Merger + +## Purpose + +This package try to merge two tracking objects from different sensor. + +## Inner-workings / Algorithms + +Merging tracking objects from different sensor is a combination of data association and state fusion algorithms. + +Detailed process depends on the merger policy. + +### decorative_tracker_merger + +In decorative_tracker_merger, we assume there are dominant tracking objects and sub tracking objects. +The name `decorative` means that sub tracking objects are used to complement the main objects. + +Usually the dominant tracking objects are from LiDAR and sub tracking objects are from Radar or Camera. + +Here show the processing pipeline. + +![decorative_tracker_merger](./image/decorative_tracker_merger.drawio.svg) + +#### time sync + +Sub object(Radar or Camera) often has higher frequency than dominant object(LiDAR). So we need to sync the time of sub object to dominant object. + +![time sync](image/time_sync.drawio.svg) + +#### data association + +In the data association, we use the following rules to determine whether two tracking objects are the same object. + +- gating + - `distance gate`: distance between two tracking objects + - `angle gate`: angle between two tracking objects + - `mahalanobis_distance_gate`: Mahalanobis distance between two tracking objects + - `min_iou_gate`: minimum IoU between two tracking objects + - `max_velocity_gate`: maximum velocity difference between two tracking objects +- score + - score used in matching is equivalent to the distance between two tracking objects + +#### tracklet update + +Sub tracking objects are merged into dominant tracking objects. + +Depends on the tracklet input sensor state, we update the tracklet state with different rules. + +| state\priority | 1st | 2nd | 3rd | +| -------------------------- | ------ | ----- | ------ | +| Kinematics except velocity | LiDAR | Radar | Camera | +| Forward velocity | Radar | LiDAR | Camera | +| Object classification | Camera | LiDAR | Radar | + +#### tracklet management + +We use the `existence_probability` to manage tracklet. + +- When we create a new tracklet, we set the `existence_probability` to $p_{sensor}$ value. +- In each update with specific sensor, we set the `existence_probability` to $p_{sensor}$ value. +- When tracklet does not have update with specific sensor, we reduce the `existence_probability` by `decay_rate` +- Object can be published if `existence_probability` is larger than `publish_probability_threshold` +- Object will be removed if `existence_probability` is smaller than `remove_probability_threshold` + +![tracklet_management](./image/tracklet_management.drawio.svg) + +These parameter can be set in `config/decorative_tracker_merger.param.yaml`. + +```yaml +tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.6 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.6 + decay_rate: 0.1 + max_dt: 1.0 +``` + +#### input/parameters + +| topic name | message type | description | +| ------------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------- | +| `~input/main_object` | `autoware_auto_perception_msgs::TrackedObjects` | Dominant tracking objects. Output will be published with this dominant object stamps. | +| `~input/sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Sub tracking objects. | +| `output/object` | `autoware_auto_perception_msgs::TrackedObjects` | Merged tracking objects. | +| `debug/interpolated_sub_object` | `autoware_auto_perception_msgs::TrackedObjects` | Interpolated sub tracking objects. | + +Default parameters are set in [config/decorative_tracker_merger.param.yaml](./config/decorative_tracker_merger.param.yaml). + +| parameter name | description | default value | +| ------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `base_link_frame_id` | base link frame id. This is used to transform the tracking object. | "base_link" | +| `time_sync_threshold` | time sync threshold. If the time difference between two tracking objects is smaller than this value, we consider these two tracking objects are the same object. | 0.05 | +| `sub_object_timeout_sec` | sub object timeout. If the sub object is not updated for this time, we consider this object is not exist. | 0.5 | +| `main_sensor_type` | main sensor type. This is used to determine the dominant tracking object. | "lidar" | +| `sub_sensor_type` | sub sensor type. This is used to determine the sub tracking object. | "radar" | +| `tracker_state_parameter` | tracker state parameter. This is used to manage the tracklet. | | + +- the detail of `tracker_state_parameter` is described in [tracklet management](#tracklet-management) + +#### tuning + +As explained in [tracklet management](#tracklet-management), this tracker merger tend to maintain the both input tracking objects. + +If there are many false positive tracking objects, + +- decrease `default__existence_probability` of that sensor +- increase `decay_rate` +- increase `publish_probability_threshold` to publish only reliable tracking objects + +### equivalent_tracker_merger + +This is future work. diff --git a/perception/tracking_object_merger/config/data_association_matrix.param.yaml b/perception/tracking_object_merger/config/data_association_matrix.param.yaml new file mode 100644 index 0000000000000..702809b3cede1 --- /dev/null +++ b/perception/tracking_object_merger/config/data_association_matrix.param.yaml @@ -0,0 +1,168 @@ +/**: + ros__parameters: + lidar-lidar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, #UNKNOWN + 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS + 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # If value is negative, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN + 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS + 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] #PEDESTRIAN + + lidar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 5.5, 6.0, 6.0, 6.0, 1.0, 1.0, 1.0, #CAR + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #BUS + 5.0, 6.0, 6.5, 6.5, 6.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN + + radar-radar: + can_assign_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 1, 1, 1, 1, 0, 0, 0, #CAR + 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK + 0, 1, 1, 1, 1, 0, 0, 0, #BUS + 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE + 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE + 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN + + max_dist_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [4.0, 4.0, 5.0, 5.0, 5.0, 3.5, 3.5, 3.5, #UNKNOWN + 4.0, 7.0, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #CAR + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRUCK + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #BUS + 5.0, 7.5, 7.5, 7.5, 7.5, 1.0, 1.0, 1.0, #TRAILER + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #MOTORBIKE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, #BICYCLE + 3.5, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.5] #PEDESTRIAN + max_rad_matrix: # If value is greater than pi, it will be ignored. + #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN + [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS + 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE + 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN + + max_velocity_diff_matrix: # Ignored when value is larger than 100.0 + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #UNKNOWN + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #CAR + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRUCK + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #BUS + 100.0, 8.0, 8.0, 8.0, 8.0, 100.0, 100.0, 100.0, #TRAILER + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #MOTORBIKE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0, #BICYCLE + 100.0, 100.0,100.0,100.0,100.0, 100.0, 100.0, 100.0] #PEDESTRIAN + + min_iou_matrix: # set all value to 0.0 to disable this constraint + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #UNKNOWN + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #CAR + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRUCK + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BUS + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #TRAILER + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #MOTORBIKE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, #BICYCLE + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] #PEDESTRIAN diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml new file mode 100644 index 0000000000000..4a108be657a27 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger.param.yaml @@ -0,0 +1,26 @@ +# Node parameters +/**: + ros__parameters: + base_link_frame_id: "base_link" + time_sync_threshold: 0.999 + sub_object_timeout_sec: 0.8 + publish_interpolated_sub_objects: true #for debug + + # choose the input sensor type for each object type + # "lidar", "radar", "camera" are available + main_sensor_type: "lidar" + sub_sensor_type: "radar" + + # tracker settings + tracker_state_parameter: + remove_probability_threshold: 0.3 + publish_probability_threshold: 0.5 + default_lidar_existence_probability: 0.7 + default_radar_existence_probability: 0.6 + default_camera_existence_probability: 0.5 + decay_rate: 0.1 + max_dt: 1.0 + + # logging settings + enable_logging: false + log_file_path: "/tmp/decorative_tracker_merger.log" diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml new file mode 100644 index 0000000000000..0b98e1b202957 --- /dev/null +++ b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml @@ -0,0 +1,16 @@ +# Merger policy for decorative tracker merger node +# decorative tracker merger works by merging the sub-object trackers into a main object tracker result +# There are 3 merger policy: +# 1. "skip": skip the sub-object tracker result +# 2. "overwrite": overwrite the main object tracker result with sub-object tracker result +# 3. "fusion": merge the main object tracker result with sub-object tracker result by using covariance based fusion +/**: + ros__parameters: + kinematics_to_be_merged: "velocity" # currently only support "velocity" + + # choose the merger policy for each object type + # : "skip", "overwrite", "fusion" + kinematics_merge_policy: "overwrite" + classification_merge_policy: "skip" + existence_prob_merge_policy: "skip" + shape_merge_policy: "skip" diff --git a/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg new file mode 100644 index 0000000000000..3e35fa4d1c338 --- /dev/null +++ b/perception/tracking_object_merger/image/decorative_tracker_merger.drawio.svg @@ -0,0 +1,345 @@ + + + + + + + + + + + + +
+
+
+ main_object +
+
+
+ + main_object + + + + + + +
+
+
+ decorative_tracker_merger_node +
+
+
+ + decorative_tracker_merger_node + + + + + + + + +
+
+
+ sub_object +
+
+
+ + sub_object + + + + + + + + +
+
+
+ time-sync +
+ (delay compensation) +
+
+
+ + time-sync... + + + + + + + + +
+
+
+ buffer +
+
+
+ + buffer + + + + + + +
+
+
+ main obj +
+ timestamp +
+
+
+ + main obj... + + + + + + + + + + +
+
+
+ inner +
+ tracklet +
+
+
+ + inner... + + + + + + + +
+
+
+ not +
+ found +
+
+
+ + not... + + + + + + + +
+
+
+ found +
+
+
+ + found + + + + + + +
+
+
+ association +
+
+
+ + association + + + + + + + + +
+
+
+ update +
+ tracklet +
+
+
+ + update... + + + + + + + + +
+
+
+ add new +
+ tracklet +
+
+
+ + add new... + + + + + + + +
+
+
+ converted +
+ tracked object +
+
+
+ + converted... + + + + + + + + +
+
+
+ tracklet management +
+ (publisher) +
+
+
+ + tracklet management... + + + + + + +
+
+
+ merged object +
+
+
+ + merged object + + + + + + +
+
+
msg: TrackedObjects
+
+
+ + msg: TrackedObjects + + + + + + +
+
+
+ remove when low existence +
+ probability objects +
+
+
+ + remove when low existence... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/time_sync.drawio.svg b/perception/tracking_object_merger/image/time_sync.drawio.svg new file mode 100644 index 0000000000000..dad2eb1a7fd27 --- /dev/null +++ b/perception/tracking_object_merger/image/time_sync.drawio.svg @@ -0,0 +1,223 @@ + + + + + + + + + + + +
+
+
time
+
+
+ + time + + + + + + + +
+
+
main object topic
+
+
+ + main object topic + + + + + + +
+
+
sub object topic
+
+
+ + sub object topic + + + + + + + + + + + + + + + + + + + + +
+
+
publish timing
+
+
+ + publish timing + + + + + + +
+
+
+ interpolated sub object +
+
+
+ + interpolated sub object + + + + + + + + +
+
+
raw main/sub object
+
+
+ + raw main/sub object + + + + + + + + + +
+
+
+ time sync threshold +
+
+
+ + time sync threshold + + + + + + +
+
+
+ time synchronize prediction pattern +
+
+
+ + time synchronize prediction pattern + + + + + + + + + +
+
+
+ 1. forward +
+
+
+ 2. backward +
+
+
+ 3. interpolation +
+
+
+
+ + 1. forward... + + + + + + + + + + + + + + + + + + +
+
+
legend
+
+
+ + legend + + + + + + +
+
+
+ disabled +
+ now +
+
+
+ + disabled... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/image/tracklet_management.drawio.svg b/perception/tracking_object_merger/image/tracklet_management.drawio.svg new file mode 100644 index 0000000000000..4f2a00dfd4dfd --- /dev/null +++ b/perception/tracking_object_merger/image/tracklet_management.drawio.svg @@ -0,0 +1,216 @@ + + + + + + + + +
+
+
0.0
+
+
+ + 0.0 + + + + + + +
+
+
1.0
+
+
+ + 1.0 + + + + + + +
+
+
Existence Probability
+
+
+ + Existence Probability + + + + + + + +
+
+
remove threshold
+
+
+ + remove threshold + + + + + + +
+
+
can publish threshold
+
+
+ + can publish threshold + + + + + + + + + +
+
+
+ decay when +
+ not observed +
+
+
+ + decay when... + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + +
+
+
+ init or update probability +
+ from sensor +
+
+
+ + init or update probabilit... + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp new file mode 100644 index 0000000000000..05fc9f6caccb5 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp @@ -0,0 +1,73 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Yukihiro Saito +// + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ + +// #include // for debug json library + +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" + +#include +#include + +#include +#include + +#include + +class DataAssociation +{ +private: + Eigen::MatrixXi can_assign_matrix_; + Eigen::MatrixXd max_dist_matrix_; + Eigen::MatrixXd max_rad_matrix_; + Eigen::MatrixXd min_iou_matrix_; + Eigen::MatrixXd max_velocity_diff_matrix_; + const double score_threshold_; + std::unique_ptr gnn_solver_ptr_; + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector); + void assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1); + Eigen::MatrixXd calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers); + double calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const; + virtual ~DataAssociation() {} +}; + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp new file mode 100644 index 0000000000000..31240848f1977 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver.hpp @@ -0,0 +1,22 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" +#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp new file mode 100644 index 0000000000000..e915b5d2a9e7b --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/gnn_solver_interface.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ + +#include +#include + +namespace gnn_solver +{ +class GnnSolverInterface +{ +public: + GnnSolverInterface() = default; + virtual ~GnnSolverInterface() = default; + + virtual void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) = 0; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__GNN_SOLVER_INTERFACE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp new file mode 100644 index 0000000000000..5c0ebc04fdde3 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class MuSSP : public GnnSolverInterface +{ +public: + MuSSP() = default; + ~MuSSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__MU_SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp new file mode 100644 index 0000000000000..47a737cf58298 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/solver/successive_shortest_path.hpp @@ -0,0 +1,37 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ +#define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ + +#include "tracking_object_merger/data_association/solver/gnn_solver_interface.hpp" + +#include +#include + +namespace gnn_solver +{ +class SSP : public GnnSolverInterface +{ +public: + SSP() = default; + ~SSP() = default; + + void maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) override; +}; +} // namespace gnn_solver + +#endif // TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__SOLVER__SUCCESSIVE_SHORTEST_PATH_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp new file mode 100644 index 0000000000000..0509fb2a07dc5 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp @@ -0,0 +1,134 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ +#define TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ + +#include "tracking_object_merger/data_association/data_association.hpp" +#include "tracking_object_merger/utils/tracker_state.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace tracking_object_merger +{ + +class DecorativeTrackerMergerNode : public rclcpp::Node +{ +public: + explicit DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options); + enum class PriorityMode : int { Object0 = 0, Object1 = 1, Confidence = 2 }; + +private: + void set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map); + + void mainObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + void subObjectsCallback( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + + bool decorativeMerger( + const MEASUREMENT_STATE input_index, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg); + autoware_auto_perception_msgs::msg::TrackedObjects predictFutureState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg, + const std_msgs::msg::Header & header); + std::optional interpolateObjectState( + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg1, + const autoware_auto_perception_msgs::msg::TrackedObjects::ConstSharedPtr & input_object_msg2, + const std_msgs::msg::Header & header); + TrackedObjects getTrackedObjects(const std_msgs::msg::Header & header); + TrackerState createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object); + +private: + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::Publisher::SharedPtr + merged_object_pub_; + rclcpp::Subscription::SharedPtr + sub_main_objects_; + rclcpp::Subscription::SharedPtr + sub_sub_objects_; + // debug object publisher + rclcpp::Publisher::SharedPtr + debug_object_pub_; + bool publish_interpolated_sub_objects_; + + /* handle objects */ + std::unordered_map> + input_merger_map_; + MEASUREMENT_STATE main_sensor_type_; + MEASUREMENT_STATE sub_sensor_type_; + std::vector inner_tracker_objects_; + std::unordered_map> data_association_map_; + std::string target_frame_; + std::string base_link_frame_id_; + // buffer to save the sub objects + std::vector + sub_objects_buffer_; + double sub_object_timeout_sec_; + double time_sync_threshold_; + + // tracker default settings + TrackerStateParameter tracker_state_parameter_; + + // merge policy (currently not used) + struct + { + std::string kinematics_to_be_merged; + merger_utils::MergePolicy kinematics_merge_policy; + merger_utils::MergePolicy classification_merge_policy; + merger_utils::MergePolicy existence_prob_merge_policy; + merger_utils::MergePolicy shape_merge_policy; + } merger_policy_params_; + + std::map merger_policy_map_ = { + {"skip", merger_utils::MergePolicy::SKIP}, + {"overwrite", merger_utils::MergePolicy::OVERWRITE}, + {"fusion", merger_utils::MergePolicy::FUSION}}; + + // debug parameters + struct logging + { + bool enable = false; + std::string path; + } logging_; +}; + +} // namespace tracking_object_merger + +#endif // TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp new file mode 100644 index 0000000000000..6bedf7db8d727 --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp @@ -0,0 +1,148 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// Enum classes to distinguish input sources +enum class MEASUREMENT_STATE : int { + NONE = 0, // 0000 + LIDAR = 1, // 0001 + RADAR = 2, // 0010 + CAMERA = 4, // 0100 + LIDAR_RADAR = 3, // 0011 + LIDAR_CAMERA = 5, // 0101 + RADAR_CAMERA = 6, // 0110 + LIDAR_RADAR_CAMERA = 7, // 0111 +}; + +// Operator overloading for MEASUREMENT_STATE +// 1. operator| for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR | MEASUREMENT_STATE::RADAR == MEASUREMENT_STATE::LIDAR_RADAR +// 2. operator& for MEASUREMENT_STATE +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::LIDAR == true +// e.g. MEASUREMENT_STATE::LIDAR_RADAR & MEASUREMENT_STATE::CAMERA == false +inline MEASUREMENT_STATE operator|(MEASUREMENT_STATE lhs, MEASUREMENT_STATE rhs) +{ + return static_cast(static_cast(lhs) | static_cast(rhs)); +} +inline bool operator&(MEASUREMENT_STATE combined, MEASUREMENT_STATE target) +{ + return (static_cast(combined) & static_cast(target)) != 0; +} + +// Struct to handle tracker state public parameter +struct TrackerStateParameter +{ + double publish_probability_threshold = 0.5; + double remove_probability_threshold = 0.3; + double default_lidar_existence_probability = 0.8; + double default_radar_existence_probability = 0.7; + double default_camera_existence_probability = 0.6; + double decay_rate = 0.1; + double max_dt = 2.0; +}; + +// Class to handle tracker state +class TrackerState +{ +private: + /* data */ + TrackedObject tracked_object_; + rclcpp::Time last_update_time_; + // Eigen::MatrixXf state_matrix_; + // Eigen::MatrixXf covariance_matrix_; + + // timer handle + std::unordered_map last_updated_time_map_; + double max_dt_ = 2.0; + +public: + TrackerState( + const MEASUREMENT_STATE input, const rclcpp::Time & last_update_time, + const TrackedObject & tracked_object); + TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid); + + ~TrackerState(); + +public: + void setParameter(const TrackerStateParameter & parameter); + bool predict(const rclcpp::Time & time); + bool predict(const double dt, std::function func); + MEASUREMENT_STATE getCurrentMeasurementState(const rclcpp::Time & current_time) const; + bool updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object); + void updateWithLIDAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithRADAR(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithCAMERA(const rclcpp::Time & current_time, const TrackedObject & tracked_object); + void updateWithoutSensor(const rclcpp::Time & current_time); + bool update(const MEASUREMENT_STATE input, const TrackedObject & tracked_object); + bool updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object, + std::function update_func); + // const functions + bool hasUUID(const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const; + bool isValid() const; + bool canPublish() const; + TrackedObject getObject() const; + +public: + // handle uuid + unique_identifier_msgs::msg::UUID const_uuid_; + // each sensor input to uuid map + std::unordered_map> + input_uuid_map_; + MEASUREMENT_STATE measurement_state_; + + // following lifecycle control parameter is overwritten by TrackerStateParameter + std::unordered_map default_existence_probability_map_ = { + {MEASUREMENT_STATE::LIDAR, 0.8}, + {MEASUREMENT_STATE::RADAR, 0.7}, + {MEASUREMENT_STATE::CAMERA, 0.6}, + }; + double existence_probability_ = 0.0; + double publish_probability_threshold_ = 0.5; + double remove_probability_threshold_ = 0.3; + double decay_rate_ = 0.1; +}; + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & time); + +#endif // TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_ diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp new file mode 100644 index 0000000000000..013040d15bded --- /dev/null +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -0,0 +1,129 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// + +#ifndef TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ +#define TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ + +// #include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ +enum MSG_COV_IDX { + X_X = 0, + X_Y = 1, + X_Z = 2, + X_ROLL = 3, + X_PITCH = 4, + X_YAW = 5, + Y_X = 6, + Y_Y = 7, + Y_Z = 8, + Y_ROLL = 9, + Y_PITCH = 10, + Y_YAW = 11, + Z_X = 12, + Z_Y = 13, + Z_Z = 14, + Z_ROLL = 15, + Z_PITCH = 16, + Z_YAW = 17, + ROLL_X = 18, + ROLL_Y = 19, + ROLL_Z = 20, + ROLL_ROLL = 21, + ROLL_PITCH = 22, + ROLL_YAW = 23, + PITCH_X = 24, + PITCH_Y = 25, + PITCH_Z = 26, + PITCH_ROLL = 27, + PITCH_PITCH = 28, + PITCH_YAW = 29, + YAW_X = 30, + YAW_Y = 31, + YAW_Z = 32, + YAW_ROLL = 33, + YAW_PITCH = 34, + YAW_YAW = 35 +}; + +// linear interpolation for tracked objects +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2); + +// predict tracked objects +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt); + +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header); + +// predict tracked objects +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header); + +} // namespace utils + +namespace merger_utils +{ +// merge policy +enum MergePolicy : int { SKIP = 0, OVERWRITE = 1, FUSION = 2 }; + +// object kinematics velocity merger +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy); + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy); + +// update tracked object +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj); + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj); + +} // namespace merger_utils + +#endif // TRACKING_OBJECT_MERGER__UTILS__UTILS_HPP_ diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml new file mode 100644 index 0000000000000..5affbe474e8ae --- /dev/null +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/tracking_object_merger/package.xml b/perception/tracking_object_merger/package.xml new file mode 100644 index 0000000000000..d74a8449b20e6 --- /dev/null +++ b/perception/tracking_object_merger/package.xml @@ -0,0 +1,32 @@ + + + + tracking_object_merger + 0.0.0 + merge tracking object + Yukihiro Saito + Yoshi Ri + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_perception_msgs + eigen + mussp + object_recognition_utils + rclcpp + rclcpp_components + tf2 + tf2_ros + tier4_autoware_utils + tier4_perception_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp new file mode 100644 index 0000000000000..e7ab6077250f8 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -0,0 +1,238 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracking_object_merger/data_association/data_association.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tracking_object_merger/data_association/solver/gnn_solver.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include +#include +#include +#include +namespace +{ +double getFormedYawAngle( + const geometry_msgs::msg::Quaternion & quat0, const geometry_msgs::msg::Quaternion & quat1, + const bool distinguish_front_or_back = true) +{ + const double yaw0 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat0)); + const double yaw1 = tier4_autoware_utils::normalizeRadian(tf2::getYaw(quat1)); + const double angle_range = distinguish_front_or_back ? M_PI : M_PI_2; + const double angle_step = distinguish_front_or_back ? 2.0 * M_PI : M_PI; + // Fixed yaw0 to be in the range of +-90 or 180 degrees of yaw1 + double fixed_yaw0 = yaw0; + while (angle_range <= yaw1 - fixed_yaw0) { + fixed_yaw0 = fixed_yaw0 + angle_step; + } + while (angle_range <= fixed_yaw0 - yaw1) { + fixed_yaw0 = fixed_yaw0 - angle_step; + } + return std::fabs(fixed_yaw0 - yaw1); +} +} // namespace + +DataAssociation::DataAssociation( + std::vector can_assign_vector, std::vector max_dist_vector, + std::vector max_rad_vector, std::vector min_iou_vector, + std::vector max_velocity_diff_vector) +: score_threshold_(0.01) +{ + { + const int assign_label_num = static_cast(std::sqrt(can_assign_vector.size())); + Eigen::Map can_assign_matrix_tmp( + can_assign_vector.data(), assign_label_num, assign_label_num); + can_assign_matrix_ = can_assign_matrix_tmp.transpose(); + } + { + const int max_dist_label_num = static_cast(std::sqrt(max_dist_vector.size())); + Eigen::Map max_dist_matrix_tmp( + max_dist_vector.data(), max_dist_label_num, max_dist_label_num); + max_dist_matrix_ = max_dist_matrix_tmp.transpose(); + } + { + const int max_rad_label_num = static_cast(std::sqrt(max_rad_vector.size())); + Eigen::Map max_rad_matrix_tmp( + max_rad_vector.data(), max_rad_label_num, max_rad_label_num); + max_rad_matrix_ = max_rad_matrix_tmp.transpose(); + } + { + const int min_iou_label_num = static_cast(std::sqrt(min_iou_vector.size())); + Eigen::Map min_iou_matrix_tmp( + min_iou_vector.data(), min_iou_label_num, min_iou_label_num); + min_iou_matrix_ = min_iou_matrix_tmp.transpose(); + } + { + const int max_velocity_diff_label_num = + static_cast(std::sqrt(max_velocity_diff_vector.size())); + Eigen::Map max_velocity_diff_matrix_tmp( + max_velocity_diff_vector.data(), max_velocity_diff_label_num, max_velocity_diff_label_num); + max_velocity_diff_matrix_ = max_velocity_diff_matrix_tmp.transpose(); + } + + gnn_solver_ptr_ = std::make_unique(); +} + +void DataAssociation::assign( + const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, + std::unordered_map & reverse_assignment) +{ + std::vector> score(src.rows()); + for (int row = 0; row < src.rows(); ++row) { + score.at(row).resize(src.cols()); + for (int col = 0; col < src.cols(); ++col) { + score.at(row).at(col) = src(row, col); + } + } + // Solve + gnn_solver_ptr_->maximizeLinearAssignment(score, &direct_assignment, &reverse_assignment); + + for (auto itr = direct_assignment.begin(); itr != direct_assignment.end();) { + if (src(itr->first, itr->second) < score_threshold_) { + itr = direct_assignment.erase(itr); + continue; + } else { + ++itr; + } + } + for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end();) { + if (src(itr->second, itr->first) < score_threshold_) { + itr = reverse_assignment.erase(itr); + continue; + } else { + ++itr; + } + } +} + +/** + * @brief calc score matrix between two tracked objects + * This is used to associate input measurements + * + * @param objects0 : measurements + * @param objects1 : base objects(tracker objects) + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects1) +{ + Eigen::MatrixXd score_matrix = + Eigen::MatrixXd::Zero(objects1.objects.size(), objects0.objects.size()); + for (size_t objects1_idx = 0; objects1_idx < objects1.objects.size(); ++objects1_idx) { + const auto & object1 = objects1.objects.at(objects1_idx); + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(objects1_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +/** + * @brief calc score matrix between two tracked object and Tracker State + * + * @param objects0 : measurements + * @param objects1 : tracker inner objects + * @return Eigen::MatrixXd + */ +Eigen::MatrixXd DataAssociation::calcScoreMatrix( + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers) +{ + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & object1 = trackers.at(trackers_idx).getObject(); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + const double score = calcScoreBetweenObjects(object0, object1); + + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +double DataAssociation::calcScoreBetweenObjects( + const autoware_auto_perception_msgs::msg::TrackedObject & object0, + const autoware_auto_perception_msgs::msg::TrackedObject & object1) const +{ + const std::uint8_t object1_label = + object_recognition_utils::getHighestProbLabel(object1.classification); + const std::uint8_t object0_label = + object_recognition_utils::getHighestProbLabel(object0.classification); + + std::vector tracker_pose = { + object1.kinematics.pose_with_covariance.pose.position.x, + object1.kinematics.pose_with_covariance.pose.position.y}; + std::vector measurement_pose = { + object0.kinematics.pose_with_covariance.pose.position.x, + object0.kinematics.pose_with_covariance.pose.position.y}; + + double score = 0.0; + if (can_assign_matrix_(object1_label, object0_label)) { + const double max_dist = max_dist_matrix_(object1_label, object0_label); + const double dist = tier4_autoware_utils::calcDistance2d( + object0.kinematics.pose_with_covariance.pose.position, + object1.kinematics.pose_with_covariance.pose.position); + + bool passed_gate = true; + // dist gate + if (passed_gate) { + if (max_dist < dist) passed_gate = false; + } + // angle gate + if (passed_gate) { + const double max_rad = max_rad_matrix_(object1_label, object0_label); + const double angle = getFormedYawAngle( + object0.kinematics.pose_with_covariance.pose.orientation, + object1.kinematics.pose_with_covariance.pose.orientation, false); + if (std::fabs(max_rad) < M_PI && std::fabs(max_rad) < std::fabs(angle)) passed_gate = false; + } + // 2d iou gate + if (passed_gate) { + const double min_iou = min_iou_matrix_(object1_label, object0_label); + const double min_union_iou_area = 1e-2; + const double iou = object_recognition_utils::get2dIoU(object0, object1, min_union_iou_area); + if (iou < min_iou) passed_gate = false; + } + // max velocity diff gate + if (passed_gate) { + const double max_velocity_diff = max_velocity_diff_matrix_(object1_label, object0_label); + // calc absolute velocity diff (only thinking about speed) + const auto speed0 = std::hypot( + object0.kinematics.twist_with_covariance.twist.linear.x, + object0.kinematics.twist_with_covariance.twist.linear.y); + const auto speed1 = std::hypot( + object1.kinematics.twist_with_covariance.twist.linear.x, + object1.kinematics.twist_with_covariance.twist.linear.y); + const double velocity_diff = std::fabs(speed0 - speed1); + if (max_velocity_diff < velocity_diff) passed_gate = false; + } + + // all gate is passed + if (passed_gate) { + score = (max_dist - std::min(dist, max_dist)) / max_dist; + if (score < score_threshold_) score = 0.0; + } + } + return score; +} diff --git a/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp new file mode 100644 index 0000000000000..fcc79c0a3cbd7 --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp @@ -0,0 +1,41 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracking_object_merger/data_association/solver/mu_successive_shortest_path.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +void MuSSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // Terminate if the graph is empty + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Solve DA by muSSP + solve_muSSP(cost, direct_assignment, reverse_assignment); +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp new file mode 100644 index 0000000000000..133f0d377373f --- /dev/null +++ b/perception/tracking_object_merger/src/data_association/successive_shortest_path/successive_shortest_path.cpp @@ -0,0 +1,370 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace gnn_solver +{ +struct ResidualEdge +{ + // Destination node + const int dst; + int capacity; + const double cost; + int flow; + // Access to the reverse edge by adjacency_list.at(dst).at(reverse) + const int reverse; + + // ResidualEdge() + // : dst(0), capacity(0), cost(0), flow(0), reverse(0) {} + + ResidualEdge(int dst, int capacity, double cost, int flow, int reverse) + : dst(dst), capacity(capacity), cost(cost), flow(flow), reverse(reverse) + { + } +}; + +void SSP::maximizeLinearAssignment( + const std::vector> & cost, std::unordered_map * direct_assignment, + std::unordered_map * reverse_assignment) +{ + // NOTE: Need to set as default arguments + bool sparse_cost = true; + // bool sparse_cost = false; + + // Hyperparameters + // double MAX_COST = 6; + const double MAX_COST = 10; + const double INF_DIST = 10000000; + const double EPS = 1e-5; + + // When there is no agents or no tasks, terminate + if (cost.size() == 0 || cost.at(0).size() == 0) { + return; + } + + // Construct a bipartite graph from the cost matrix + int n_agents = cost.size(); + int n_tasks = cost.at(0).size(); + + int n_dummies; + if (sparse_cost) { + n_dummies = n_agents; + } else { + n_dummies = 0; + } + + int source = 0; + int sink = n_agents + n_tasks + 1; + int n_nodes = n_agents + n_tasks + n_dummies + 2; + + // // Print cost matrix + // std::cout << std::endl; + // for (int agent = 0; agent < n_agents; agent++) + // { + // for (int task = 0; task < n_tasks; task++) + // { + // std::cout << cost.at(agent).at(task) << " "; + // } + // std::cout << std::endl; + // } + + // std::chrono::system_clock::time_point start_time, end_time; + // start_time = std::chrono::system_clock::now(); + + // Adjacency list of residual graph (index: nodes) + // - 0: source node + // - {1, ..., n_agents}: agent nodes + // - {n_agents+1, ..., n_agents+n_tasks}: task nodes + // - n_agents+n_tasks+1: sink node + // - {n_agents+n_tasks+2, ..., n_agents+n_tasks+1+n_agents}: + // dummy node (when sparse_cost is true) + std::vector> adjacency_list(n_nodes); + + // Reserve memory + for (int v = 0; v < n_nodes; ++v) { + if (v == source) { + // Source + adjacency_list.at(v).reserve(n_agents); + } else if (v <= n_agents) { + // Agents + adjacency_list.at(v).reserve(n_tasks + 1 + 1); + } else if (v <= n_agents + n_tasks) { + // Tasks + adjacency_list.at(v).reserve(n_agents + 1); + } else if (v == sink) { + // Sink + adjacency_list.at(v).reserve(n_tasks + n_dummies); + } else { + // Dummies + adjacency_list.at(v).reserve(2); + } + } + + // Add edges form source + for (int agent = 0; agent < n_agents; ++agent) { + // From source to agent + adjacency_list.at(source).emplace_back(agent + 1, 1, 0, 0, adjacency_list.at(agent + 1).size()); + // From agent to source + adjacency_list.at(agent + 1).emplace_back( + source, 0, 0, 0, adjacency_list.at(source).size() - 1); + } + + // Add edges from agents + for (int agent = 0; agent < n_agents; ++agent) { + for (int task = 0; task < n_tasks; ++task) { + if (!sparse_cost || cost.at(agent).at(task) > EPS) { + // From agent to task + adjacency_list.at(agent + 1).emplace_back( + task + n_agents + 1, 1, MAX_COST - cost.at(agent).at(task), 0, + adjacency_list.at(task + n_agents + 1).size()); + + // From task to agent + adjacency_list.at(task + n_agents + 1) + .emplace_back( + agent + 1, 0, cost.at(agent).at(task) - MAX_COST, 0, + adjacency_list.at(agent + 1).size() - 1); + } + } + } + + // Add edges form tasks + for (int task = 0; task < n_tasks; ++task) { + // From task to sink + adjacency_list.at(task + n_agents + 1) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to task + adjacency_list.at(sink).emplace_back( + task + n_agents + 1, 0, 0, 0, adjacency_list.at(task + n_agents + 1).size() - 1); + } + + // Add edges from dummy + if (sparse_cost) { + for (int agent = 0; agent < n_agents; ++agent) { + // From agent to dummy + adjacency_list.at(agent + 1).emplace_back( + agent + n_agents + n_tasks + 2, 1, MAX_COST, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size()); + + // From dummy to agent + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(agent + 1, 0, -MAX_COST, 0, adjacency_list.at(agent + 1).size() - 1); + + // From dummy to sink + adjacency_list.at(agent + n_agents + n_tasks + 2) + .emplace_back(sink, 1, 0, 0, adjacency_list.at(sink).size()); + + // From sink to dummy + adjacency_list.at(sink).emplace_back( + agent + n_agents + n_tasks + 2, 0, 0, 0, + adjacency_list.at(agent + n_agents + n_tasks + 2).size() - 1); + } + } + + // Maximum flow value + const int max_flow = std::min(n_agents, n_tasks); + + // Feasible potentials + std::vector potentials(n_nodes, 0); + + // Shortest path lengths + std::vector distances(n_nodes, INF_DIST); + + // Whether previously visited the node or not + std::vector is_visited(n_nodes, false); + + // Parent node () + std::vector> prev_values(n_nodes); + + for (int i = 0; i < max_flow; ++i) { + // Initialize priority queue () + std::priority_queue< + std::pair, std::vector>, + std::greater>> + p_queue; + + // Reset all trajectory states + if (i > 0) { + std::fill(distances.begin(), distances.end(), INF_DIST); + std::fill(is_visited.begin(), is_visited.end(), false); + } + + // Start trajectory from the source node + p_queue.push(std::make_pair(0, source)); + distances.at(source) = 0; + + while (!p_queue.empty()) { + // Get the next element + std::pair cur_elem = p_queue.top(); + // std::cout << "[pop]: (" << cur_elem.first << ", " << cur_elem.second << ")" << std::endl; + p_queue.pop(); + + double cur_node_dist = cur_elem.first; + int cur_node = cur_elem.second; + + // If already visited node, skip and continue + if (is_visited.at(cur_node)) { + continue; + } + assert(cur_node_dist == distances.at(cur_node)); + + // Mark as visited + is_visited.at(cur_node) = true; + // Update potential + potentials.at(cur_node) += cur_node_dist; + + // When reached to the sink node, terminate. + if (cur_node == sink) { + break; + } + + // Loop over the incident nodes(/edges) + for (auto it_incident_edge = adjacency_list.at(cur_node).cbegin(); + it_incident_edge != adjacency_list.at(cur_node).cend(); it_incident_edge++) { + // If the node is not visited and have capacity to increase flow, visit. + if (!is_visited.at(it_incident_edge->dst) && it_incident_edge->capacity > 0) { + // Calculate reduced cost + double reduced_cost = + it_incident_edge->cost + potentials.at(cur_node) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + if (distances.at(it_incident_edge->dst) > reduced_cost) { + distances.at(it_incident_edge->dst) = reduced_cost; + prev_values.at(it_incident_edge->dst) = + std::make_pair(cur_node, it_incident_edge - adjacency_list.at(cur_node).cbegin()); + // std::cout << "[push]: (" << reduced_cost << ", " << next_v << ")" << std::endl; + p_queue.push(std::make_pair(reduced_cost, it_incident_edge->dst)); + } + } + } + } + + // Shortest path length to sink is greater than MAX_COST, + // which means no non-dummy routes left, terminate + if (potentials.at(sink) >= MAX_COST) { + break; + } + + // Update potentials of unvisited nodes + for (int v = 0; v < n_nodes; ++v) { + if (!is_visited.at(v)) { + potentials.at(v) += distances.at(sink); + } + } + // //Print potentials + // for (int v = 0; v < n_nodes; ++v) + // { + // std::cout << potentials.at(v) << ", "; + // } + // std::cout << std::endl; + + // Increase/decrease flow and capacity along the shortest path from the source to the sink + int v = sink; + int prev_v; + while (v != source) { + ResidualEdge & e_forward = + adjacency_list.at(prev_values.at(v).first).at(prev_values.at(v).second); + assert(e_forward.dst == v); + ResidualEdge & e_backward = adjacency_list.at(v).at(e_forward.reverse); + prev_v = e_backward.dst; + + if (e_backward.flow == 0) { + // Increase flow + // State A + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 0); + + e_forward.capacity -= 1; + e_forward.flow += 1; + e_backward.capacity += 1; + + // State B + assert(e_forward.capacity == 0); + assert(e_forward.flow == 1); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } else { + // Decrease flow + // State B + assert(e_forward.capacity == 1); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 0); + assert(e_backward.flow == 1); + + e_forward.capacity -= 1; + e_backward.capacity += 1; + e_backward.flow -= 1; + + // State A + assert(e_forward.capacity == 0); + assert(e_forward.flow == 0); + assert(e_backward.capacity == 1); + assert(e_backward.flow == 0); + } + + v = prev_v; + } + +#ifndef NDEBUG + // Check if the potentials are feasible potentials + for (int v = 0; v < n_nodes; ++v) { + for (auto it_incident_edge = adjacency_list.at(v).cbegin(); + it_incident_edge != adjacency_list.at(v).cend(); ++it_incident_edge) { + if (it_incident_edge->capacity > 0) { + double reduced_cost = + it_incident_edge->cost + potentials.at(v) - potentials.at(it_incident_edge->dst); + assert(reduced_cost >= 0); + } + } + } +#endif + } + + // Output + for (int agent = 0; agent < n_agents; ++agent) { + for (auto it_incident_edge = adjacency_list.at(agent + 1).cbegin(); + it_incident_edge != adjacency_list.at(agent + 1).cend(); ++it_incident_edge) { + int task = it_incident_edge->dst - (n_agents + 1); + + // If the flow value is 1 and task is not dummy, assign the task to the agent. + if (it_incident_edge->flow == 1 && 0 <= task && task < n_tasks) { + (*direct_assignment)[agent] = task; + (*reverse_assignment)[task] = agent; + break; + } + } + } + +#ifndef NDEBUG + // Check if the result is valid assignment + for (int agent = 0; agent < n_agents; ++agent) { + if (direct_assignment->find(agent) != direct_assignment->cend()) { + int task = (*direct_assignment).at(agent); + assert(direct_assignment->at(agent) == task); + assert(reverse_assignment->at(task) == agent); + } + } +#endif +} +} // namespace gnn_solver diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp new file mode 100644 index 0000000000000..47a333691eabf --- /dev/null +++ b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp @@ -0,0 +1,403 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracking_object_merger/decorative_tracker_merger.hpp" + +#include "object_recognition_utils/object_recognition_utils.hpp" +#include "tracking_object_merger/data_association/solver/successive_shortest_path.hpp" +#include "tracking_object_merger/utils/utils.hpp" + +#include + +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +namespace tracking_object_merger +{ + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +// get unix time from header +double getUnixTime(const std_msgs::msg::Header & header) +{ + return header.stamp.sec + header.stamp.nanosec * 1e-9; +} + +// calc association score matrix +Eigen::MatrixXd calcScoreMatrixForAssociation( + const MEASUREMENT_STATE measurement_state, + const autoware_auto_perception_msgs::msg::TrackedObjects & objects0, + const std::vector & trackers, + const std::unordered_map> & data_association_map + // const bool debug_log, const std::string & file_name // do not logging for now +) +{ + // get current time + const rclcpp::Time current_time = rclcpp::Time(objects0.header.stamp); + + // calc score matrix + Eigen::MatrixXd score_matrix = Eigen::MatrixXd::Zero(trackers.size(), objects0.objects.size()); + for (size_t trackers_idx = 0; trackers_idx < trackers.size(); ++trackers_idx) { + const auto & tracker_obj = trackers.at(trackers_idx); + const auto & object1 = tracker_obj.getObject(); + const auto & tracker_state = tracker_obj.getCurrentMeasurementState(current_time); + + for (size_t objects0_idx = 0; objects0_idx < objects0.objects.size(); ++objects0_idx) { + const auto & object0 = objects0.objects.at(objects0_idx); + // switch calc score function by input and trackers measurement state + // we assume that lidar and radar are exclusive + double score; + const auto input_has_lidar = measurement_state & MEASUREMENT_STATE::LIDAR; + const auto tracker_has_lidar = tracker_state & MEASUREMENT_STATE::LIDAR; + if (input_has_lidar && tracker_has_lidar) { + score = data_association_map.at("lidar-lidar")->calcScoreBetweenObjects(object0, object1); + } else if (!input_has_lidar && !tracker_has_lidar) { + score = data_association_map.at("radar-radar")->calcScoreBetweenObjects(object0, object1); + } else { + score = data_association_map.at("lidar-radar")->calcScoreBetweenObjects(object0, object1); + } + score_matrix(trackers_idx, objects0_idx) = score; + } + } + return score_matrix; +} + +DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("decorative_object_merger_node", node_options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_) +{ + // Subscriber + sub_main_objects_ = create_subscription( + "input/main_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::mainObjectsCallback, this, std::placeholders::_1)); + sub_sub_objects_ = create_subscription( + "input/sub_object", rclcpp::QoS{1}, + std::bind(&DecorativeTrackerMergerNode::subObjectsCallback, this, std::placeholders::_1)); + + // merged object publisher + merged_object_pub_ = create_publisher("output/object", rclcpp::QoS{1}); + // debug object publisher + debug_object_pub_ = + create_publisher("debug/interpolated_sub_object", rclcpp::QoS{1}); + + // logging + logging_.enable = declare_parameter("enable_logging"); + logging_.path = declare_parameter("logging_file_path"); + + // Parameters + publish_interpolated_sub_objects_ = declare_parameter("publish_interpolated_sub_objects"); + base_link_frame_id_ = declare_parameter("base_link_frame_id"); + time_sync_threshold_ = declare_parameter("time_sync_threshold"); + sub_object_timeout_sec_ = declare_parameter("sub_object_timeout_sec"); + // default setting parameter for tracker + tracker_state_parameter_.remove_probability_threshold = + declare_parameter("tracker_state_parameter.remove_probability_threshold"); + tracker_state_parameter_.publish_probability_threshold = + declare_parameter("tracker_state_parameter.publish_probability_threshold"); + tracker_state_parameter_.default_lidar_existence_probability = + declare_parameter("tracker_state_parameter.default_lidar_existence_probability"); + tracker_state_parameter_.default_radar_existence_probability = + declare_parameter("tracker_state_parameter.default_radar_existence_probability"); + tracker_state_parameter_.default_camera_existence_probability = + declare_parameter("tracker_state_parameter.default_camera_existence_probability"); + tracker_state_parameter_.decay_rate = + declare_parameter("tracker_state_parameter.decay_rate"); + tracker_state_parameter_.max_dt = declare_parameter("tracker_state_parameter.max_dt"); + + const std::string main_sensor_type = declare_parameter("main_sensor_type"); + const std::string sub_sensor_type = declare_parameter("sub_sensor_type"); + // str to MEASUREMENT_STATE + auto str2measurement_state = [](const std::string & str) { + if (str == "lidar") { + return MEASUREMENT_STATE::LIDAR; + } else if (str == "radar") { + return MEASUREMENT_STATE::RADAR; + } else { + throw std::runtime_error("invalid sensor type"); + } + }; + main_sensor_type_ = str2measurement_state(main_sensor_type); + sub_sensor_type_ = str2measurement_state(sub_sensor_type); + + /* init association **/ + // lidar-lidar association matrix + set3dDataAssociation("lidar-lidar", data_association_map_); + // lidar-radar association matrix + set3dDataAssociation("lidar-radar", data_association_map_); + // radar-radar association matrix + set3dDataAssociation("radar-radar", data_association_map_); +} + +void DecorativeTrackerMergerNode::set3dDataAssociation( + const std::string & prefix, + std::unordered_map> & data_association_map) +{ + const auto tmp = this->declare_parameter>(prefix + ".can_assign_matrix"); + const std::vector can_assign_matrix(tmp.begin(), tmp.end()); + const auto max_dist_matrix = + this->declare_parameter>(prefix + ".max_dist_matrix"); + const auto max_rad_matrix = + this->declare_parameter>(prefix + ".max_rad_matrix"); + const auto min_iou_matrix = + this->declare_parameter>(prefix + ".min_iou_matrix"); + const auto max_velocity_diff_matrix = + this->declare_parameter>(prefix + ".max_velocity_diff_matrix"); + + data_association_map[prefix] = std::make_unique( + can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix, max_velocity_diff_matrix); +} + +/** + * @brief callback function for main objects + * + * @param main_objects + * @note if there are no sub objects, publish main objects as it is + * else, merge main objects and sub objects + */ +void DecorativeTrackerMergerNode::mainObjectsCallback( + const TrackedObjects::ConstSharedPtr & main_objects) +{ + // try to merge sub object + if (!sub_objects_buffer_.empty()) { + // get interpolated sub objects + // get newest sub objects which timestamp is earlier to main objects + TrackedObjects::ConstSharedPtr closest_time_sub_objects; + TrackedObjects::ConstSharedPtr closest_time_sub_objects_later; + for (const auto & sub_object : sub_objects_buffer_) { + if (getUnixTime(sub_object->header) < getUnixTime(main_objects->header)) { + closest_time_sub_objects = sub_object; + } else { + closest_time_sub_objects_later = sub_object; + break; + } + } + // get delay compensated sub objects + const auto interpolated_sub_objects = interpolateObjectState( + closest_time_sub_objects, closest_time_sub_objects_later, main_objects->header); + if (interpolated_sub_objects.has_value()) { + // Merge sub objects + const auto interp_sub_objs = interpolated_sub_objects.value(); + debug_object_pub_->publish(interp_sub_objs); + this->decorativeMerger( + sub_sensor_type_, std::make_shared(interpolated_sub_objects.value())); + } else { + RCLCPP_DEBUG(this->get_logger(), "interpolated_sub_objects is null"); + } + } + + // try to merge main object + this->decorativeMerger(main_sensor_type_, main_objects); + + merged_object_pub_->publish(getTrackedObjects(main_objects->header)); +} + +/** + * @brief callback function for sub objects + * + * @param msg + * @note push back sub objects to buffer and remove old sub objects + */ +void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::ConstSharedPtr & msg) +{ + sub_objects_buffer_.push_back(msg); + // remove old sub objects + // const auto now = get_clock()->now(); + const auto now = rclcpp::Time(msg->header.stamp); + const auto remove_itr = std::remove_if( + sub_objects_buffer_.begin(), sub_objects_buffer_.end(), [now, this](const auto & sub_object) { + return (now - rclcpp::Time(sub_object->header.stamp)).seconds() > sub_object_timeout_sec_; + }); + sub_objects_buffer_.erase(remove_itr, sub_objects_buffer_.end()); +} + +/** + * @brief merge objects into inner_tracker_objects_ + * + * @param main_objects + * @return TrackedObjects + * + * @note 1. interpolate sub objects to sync main objects + * 2. do association + * 3. merge objects + */ +bool DecorativeTrackerMergerNode::decorativeMerger( + const MEASUREMENT_STATE input_sensor, const TrackedObjects::ConstSharedPtr & input_objects_msg) +{ + // get current time + const auto current_time = rclcpp::Time(input_objects_msg->header.stamp); + if (input_objects_msg->objects.empty()) { + return false; + } + if (inner_tracker_objects_.empty()) { + for (const auto & object : input_objects_msg->objects) { + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object)); + } + } + + // do prediction for inner objects + for (auto & object : inner_tracker_objects_) { + object.predict(current_time); + } + + // TODO(yoshiri): pre-association + + // associate inner objects and input objects + /* global nearest neighbor */ + std::unordered_map direct_assignment, reverse_assignment; + const auto & objects1 = input_objects_msg->objects; + Eigen::MatrixXd score_matrix = calcScoreMatrixForAssociation( + input_sensor, *input_objects_msg, inner_tracker_objects_, data_association_map_); + data_association_map_.at("lidar-lidar") + ->assign(score_matrix, direct_assignment, reverse_assignment); + + // look for tracker + for (int tracker_idx = 0; tracker_idx < static_cast(inner_tracker_objects_.size()); + ++tracker_idx) { + auto & object0_state = inner_tracker_objects_.at(tracker_idx); + if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found and merge + const auto & object1 = objects1.at(direct_assignment.at(tracker_idx)); + // merge object1 into object0_state + object0_state.updateState(input_sensor, current_time, object1); + } else { // not found + // decrease existence probability + object0_state.updateWithoutSensor(current_time); + } + } + // look for new object + for (int object1_idx = 0; object1_idx < static_cast(objects1.size()); ++object1_idx) { + const auto & object1 = objects1.at(object1_idx); + if (reverse_assignment.find(object1_idx) != reverse_assignment.end()) { // found + } else { // not found + inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object1)); + } + } + return true; +} + +/** + * @brief interpolate sub objects to sync main objects + * + * @param former_msg + * @param latter_msg + * @param output_header + * @return std::optional + * + * @note 1. if both msg is nullptr, return null optional + * 2. if former_msg is nullptr, return latter_msg + * 3. if latter_msg is nullptr, return former_msg + * 4. if both msg is not nullptr, do the interpolation + */ +std::optional DecorativeTrackerMergerNode::interpolateObjectState( + const TrackedObjects::ConstSharedPtr & former_msg, + const TrackedObjects::ConstSharedPtr & latter_msg, const std_msgs::msg::Header & output_header) +{ + // Assumption: output_header must be newer than former_msg and older than latter_msg + // There three possible patterns + // 1. both msg is nullptr + // 2. former_msg is nullptr + // 3. latter_msg is nullptr + // 4. both msg is not nullptr + + // 1. both msg is nullptr + if (former_msg == nullptr && latter_msg == nullptr) { + // return null optional + RCLCPP_DEBUG(this->get_logger(), "interpolation err: both msg is nullptr"); + return std::nullopt; + } + + // 2. former_msg is nullptr + if (former_msg == nullptr) { + // std::cout << "interpolation: 2. former_msg is nullptr" << std::endl; + // depends on header stamp difference + if ( + (rclcpp::Time(latter_msg->header.stamp) - rclcpp::Time(output_header.stamp)).seconds() > + time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG( + this->get_logger(), "interpolation err: latter msg is too different from output msg"); + return std::nullopt; + } else { // else, return latter_msg + return *latter_msg; + } + + // 3. latter_msg is nullptr + } else if (latter_msg == nullptr) { + // std::cout << "interpolation: 3. latter_msg is nullptr" << std::endl; + // depends on header stamp difference + const auto dt = + (rclcpp::Time(output_header.stamp) - rclcpp::Time(former_msg->header.stamp)).seconds(); + if (dt > time_sync_threshold_) { + // do nothing + RCLCPP_DEBUG(this->get_logger(), "interpolation err: former msg is too old"); + return std::nullopt; + } else { + // else, return former_msg + return utils::predictPastOrFutureTrackedObjects(*former_msg, output_header); + } + + // 4. both msg is not nullptr + } else { + // do the interpolation + // std::cout << "interpolation: 4. both msg is not nullptr" << std::endl; + TrackedObjects interpolated_msg = + utils::interpolateTrackedObjects(*former_msg, *latter_msg, output_header); + return interpolated_msg; + } +} + +// get merged objects +TrackedObjects DecorativeTrackerMergerNode::getTrackedObjects(const std_msgs::msg::Header & header) +{ + // get main objects + rclcpp::Time current_time = rclcpp::Time(header.stamp); + return getTrackedObjectsFromTrackerStates(inner_tracker_objects_, current_time); +} + +// create new tracker +TrackerState DecorativeTrackerMergerNode::createNewTracker( + const MEASUREMENT_STATE input_index, rclcpp::Time current_time, + const autoware_auto_perception_msgs::msg::TrackedObject & input_object) +{ + // check if object id is not included in innner_tracker_objects_ + for (const auto & object : inner_tracker_objects_) { + if (object.const_uuid_ == input_object.object_id) { + // create new uuid + unique_identifier_msgs::msg::UUID uuid; + // Generate random number + std::mt19937 gen(std::random_device{}()); + std::independent_bits_engine bit_eng(gen); + std::generate(uuid.uuid.begin(), uuid.uuid.end(), bit_eng); + auto new_tracker = TrackerState(input_index, current_time, input_object, uuid); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; + } + } + // if not found, just create new tracker + auto new_tracker = TrackerState(input_index, current_time, input_object); + new_tracker.setParameter(tracker_state_parameter_); + return new_tracker; +} + +} // namespace tracking_object_merger + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(tracking_object_merger::DecorativeTrackerMergerNode) diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp new file mode 100644 index 0000000000000..f0dc657145a24 --- /dev/null +++ b/perception/tracking_object_merger/src/utils/tracker_state.cpp @@ -0,0 +1,321 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracking_object_merger/utils/tracker_state.hpp" + +#include "tracking_object_merger/utils/utils.hpp" + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +/** + * @brief Construct a new Tracker State:: Tracker State object + * + * @param input_source : input source distinguished + * @param tracked_object : input source tracked object + * @param last_update_time : last update time + */ +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(tracked_object.object_id), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +TrackerState::TrackerState( + const MEASUREMENT_STATE input_source, const rclcpp::Time & last_update_time, + const autoware_auto_perception_msgs::msg::TrackedObject & tracked_object, + const unique_identifier_msgs::msg::UUID & uuid) +: tracked_object_(tracked_object), + last_update_time_(last_update_time), + const_uuid_(uuid), + measurement_state_(input_source) +{ + input_uuid_map_[input_source] = tracked_object_.object_id; + last_updated_time_map_[input_source] = last_update_time; + existence_probability_ = default_existence_probability_map_[input_source]; +} + +void TrackerState::setParameter(const TrackerStateParameter & parameter) +{ + max_dt_ = parameter.max_dt; + publish_probability_threshold_ = parameter.publish_probability_threshold; + remove_probability_threshold_ = parameter.remove_probability_threshold; + default_existence_probability_map_.at(MEASUREMENT_STATE::LIDAR) = + parameter.default_lidar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::RADAR) = + parameter.default_radar_existence_probability; + default_existence_probability_map_.at(MEASUREMENT_STATE::CAMERA) = + parameter.default_camera_existence_probability; + decay_rate_ = parameter.decay_rate; +} + +/** + * @brief Predict state to current time + * + * @param current_time + * @return true + * @return false + */ +bool TrackerState::predict(const rclcpp::Time & current_time) +{ + // get dt and give warning if dt is too large + double dt = (current_time - last_update_time_).seconds(); + if (std::abs(dt) > max_dt_) { + std::cerr << "[tracking_object_merger] dt is too large: " << dt << std::endl; + return false; + } + + // do prediction + last_update_time_ = current_time; + return this->predict(dt, utils::predictPastOrFutureTrackedObject); +} + +/** + * @brief Predict state to current time with given update function + * + * @param dt : time to predict + * @param func : update function (e.g. PredictPastOrFutureTrackedObject) + * @return true: prediction succeeded + * @return false: prediction failed + */ +bool TrackerState::predict( + const double dt, std::function func) +{ + const auto predicted_object = func(tracked_object_, dt); + tracked_object_ = predicted_object; + return true; +} + +// get current measurement state +MEASUREMENT_STATE TrackerState::getCurrentMeasurementState(const rclcpp::Time & current_time) const +{ + MEASUREMENT_STATE measurement_state = MEASUREMENT_STATE::NONE; + // check LIDAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::LIDAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::LIDAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::LIDAR; + } + } + // check RADAR + if (last_updated_time_map_.find(MEASUREMENT_STATE::RADAR) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::RADAR)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::RADAR; + } + } + // check CAMERA + if (last_updated_time_map_.find(MEASUREMENT_STATE::CAMERA) != last_updated_time_map_.end()) { + if ((current_time - last_updated_time_map_.at(MEASUREMENT_STATE::CAMERA)).seconds() < max_dt_) { + measurement_state = measurement_state | MEASUREMENT_STATE::CAMERA; + } + } + return measurement_state; +} + +bool TrackerState::updateState( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & tracked_object) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0.0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return false; + } + + // predict + if (dt > 0.0) { + this->predict(dt, utils::predictPastOrFutureTrackedObject); + } + + // get current measurement state + measurement_state_ = getCurrentMeasurementState(current_time); + + // update with input + if (input & MEASUREMENT_STATE::LIDAR) { + updateWithLIDAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::RADAR) { + updateWithRADAR(current_time, tracked_object); + } + if (input & MEASUREMENT_STATE::CAMERA) { + updateWithCAMERA(current_time, tracked_object); + } + return true; +} + +void TrackerState::updateWithCAMERA( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::CAMERA, current_time, tracked_object, + merger_utils::updateOnlyClassification); +} + +void TrackerState::updateWithLIDAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if radar is available, do not update velocity + if (measurement_state_ & MEASUREMENT_STATE::RADAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, merger_utils::updateExceptVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::LIDAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +void TrackerState::updateWithRADAR( + const rclcpp::Time & current_time, const TrackedObject & tracked_object) +{ + // if lidar is available, update only velocity + if (measurement_state_ & MEASUREMENT_STATE::LIDAR) { + // update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateOnlyObjectVelocity); + } else { + // else just update tracked object + updateWithFunction( + MEASUREMENT_STATE::RADAR, current_time, tracked_object, + merger_utils::updateWholeTrackedObject); + } +} + +bool TrackerState::updateWithFunction( + const MEASUREMENT_STATE input, const rclcpp::Time & current_time, + const TrackedObject & input_tracked_object, + std::function update_func) +{ + // put input uuid and last update time + if (current_time > last_update_time_) { + const auto predict_successful = predict(current_time); + if (!predict_successful) { + return false; + } + } + + // update with measurement state + last_updated_time_map_[input] = current_time; + input_uuid_map_[input] = input_tracked_object.object_id; + measurement_state_ = measurement_state_ | input; + existence_probability_ = + std::max(existence_probability_, default_existence_probability_map_[input]); + + // update tracked object + update_func(tracked_object_, input_tracked_object); + tracked_object_.object_id = const_uuid_; // overwrite uuid to stay same + return true; +} + +void TrackerState::updateWithoutSensor(const rclcpp::Time & current_time) +{ + // calc dt + double dt = (current_time - last_update_time_).seconds(); + if (dt < 0) { + std::cerr << "[tracking_object_merger] dt is negative: " << dt << std::endl; + return; + } + + // predict + if (dt > 0.0) { + const auto predict_successful = this->predict(dt, utils::predictPastOrFutureTrackedObject); + if (!predict_successful) { + existence_probability_ = 0.0; // remove object + } + } + + // reduce probability + existence_probability_ -= decay_rate_; + if (existence_probability_ < 0.0) { + existence_probability_ = 0.0; + } +} + +TrackedObject TrackerState::getObject() const +{ + TrackedObject tracked_object = tracked_object_; + tracked_object.object_id = const_uuid_; + tracked_object.existence_probability = + static_cast(existence_probability_); // double -> float + return tracked_object; +} + +bool TrackerState::hasUUID( + const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const +{ + if (input_uuid_map_.find(input) == input_uuid_map_.end()) { + return false; + } + return input_uuid_map_.at(input) == uuid; +} + +bool TrackerState::isValid() const +{ + // check if tracker state is valid + if (existence_probability_ < remove_probability_threshold_) { + return false; + } + return true; +} + +bool TrackerState::canPublish() const +{ + // check if tracker state is valid + if (existence_probability_ < publish_probability_threshold_) { + return false; + } + return true; +} + +TrackerState::~TrackerState() +{ + // destructor +} + +TrackedObjects getTrackedObjectsFromTrackerStates( + std::vector & tracker_states, const rclcpp::Time & current_time) +{ + TrackedObjects tracked_objects; + + // sanitize and get tracked objects + for (auto it = tracker_states.begin(); it != tracker_states.end();) { + // check if tracker state is valid + if (it->isValid()) { + if (it->canPublish()) { + // if valid, get tracked object + tracked_objects.objects.push_back(it->getObject()); + } + ++it; + } else { + // if not valid, delete tracker state + it = tracker_states.erase(it); + } + } + + // update header + tracked_objects.header.stamp = current_time; + tracked_objects.header.frame_id = "map"; // TODO(yoshiri): get frame_id from input + return tracked_objects; +} diff --git a/perception/tracking_object_merger/src/utils/utils.cpp b/perception/tracking_object_merger/src/utils/utils.cpp new file mode 100644 index 0000000000000..5987bdc2d1bef --- /dev/null +++ b/perception/tracking_object_merger/src/utils/utils.cpp @@ -0,0 +1,403 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tracking_object_merger/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +namespace utils +{ + +/** + * @brief linear interpolation for tracked object + * + * @param obj1 : obj1 and obj2 must have same shape type + * @param obj2 : obj1 and obj2 must have same shape type + * @param weight : 0 <= weight <= 1, weight = 0: obj1, weight = 1: obj2 + * @return TrackedObject : interpolated object + */ +TrackedObject linearInterpolationForTrackedObject( + const TrackedObject & obj1, const TrackedObject & obj2, const double weight) +{ + // interpolate obj1 and obj2 with weight: obj1 * (1 - weight) + obj2 * weight + // weight = 0: obj1, weight = 1: obj2 + + // weight check (0 <= weight <= 1) + if (weight < 0 || weight > 1) { + std::cerr << "weight must be 0 <= weight <= 1 in linearInterpolationForTrackedObject function." + << std::endl; + return obj1; + } + + // output object + TrackedObject output; + // copy input object at first + output = obj1; + + // get current twist and pose + const auto twist1 = obj1.kinematics.twist_with_covariance.twist; + const auto pose1 = obj1.kinematics.pose_with_covariance.pose; + const auto twist2 = obj2.kinematics.twist_with_covariance.twist; + const auto pose2 = obj2.kinematics.pose_with_covariance.pose; + + // interpolate twist + auto & output_twist = output.kinematics.twist_with_covariance.twist; + output_twist.linear.x = twist1.linear.x * (1 - weight) + twist2.linear.x * weight; + output_twist.linear.y = twist1.linear.y * (1 - weight) + twist2.linear.y * weight; + output_twist.linear.z = twist1.linear.z * (1 - weight) + twist2.linear.z * weight; + output_twist.angular.x = twist1.angular.x * (1 - weight) + twist2.angular.x * weight; + output_twist.angular.y = twist1.angular.y * (1 - weight) + twist2.angular.y * weight; + output_twist.angular.z = twist1.angular.z * (1 - weight) + twist2.angular.z * weight; + + // interpolate pose + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x = pose1.position.x * (1 - weight) + pose2.position.x * weight; + output_pose.position.y = pose1.position.y * (1 - weight) + pose2.position.y * weight; + output_pose.position.z = pose1.position.z * (1 - weight) + pose2.position.z * weight; + // interpolate orientation with slerp + // https://en.wikipedia.org/wiki/Slerp + const auto q1 = tf2::Quaternion( + pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w); + const auto q2 = tf2::Quaternion( + pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w); + const auto q = q1.slerp(q2, weight); + output_pose.orientation.x = q.x(); + output_pose.orientation.y = q.y(); + output_pose.orientation.z = q.z(); + output_pose.orientation.w = q.w(); + + // interpolate shape(mostly for bounding box) + const auto shape1 = obj1.shape; + const auto shape2 = obj2.shape; + if (shape1.type != shape2.type) { + // if shape type is different, return obj1 + } else { + if (shape1.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + auto & output_shape = output.shape; + output_shape.dimensions.x = shape1.dimensions.x * (1 - weight) + shape2.dimensions.x * weight; + output_shape.dimensions.y = shape1.dimensions.y * (1 - weight) + shape2.dimensions.y * weight; + output_shape.dimensions.z = shape1.dimensions.z * (1 - weight) + shape2.dimensions.z * weight; + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // (TODO) implement + } else if (shape1.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // (TODO) implement + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in linearInterpolationForTrackedObject function." + << std::endl; + return output; + } + } + + return output; +} + +/** + * @brief predict past or future tracked object + * + * @param obj + * @param dt + * @return TrackedObject + */ +TrackedObject predictPastOrFutureTrackedObject(const TrackedObject & obj, const double dt) +{ + // output object + TrackedObject output; + // copy input object at first + output = obj; + if (dt == 0) { + return output; + } + + // get current twist and pose + const auto twist = obj.kinematics.twist_with_covariance.twist; + const auto pose = obj.kinematics.pose_with_covariance.pose; + + // get yaw + const auto yaw = tf2::getYaw(pose.orientation); + const auto vx = twist.linear.x; + const auto vy = twist.linear.y; + + // linear prediction equation: + // x = x0 + vx * cos(yaw) * dt - vy * sin(yaw) * dt + // y = y0 + vx * sin(yaw) * dt + vy * cos(yaw) * dt + auto & output_pose = output.kinematics.pose_with_covariance.pose; + output_pose.position.x += vx * std::cos(yaw) * dt - vy * std::sin(yaw) * dt; + output_pose.position.y += vx * std::sin(yaw) * dt + vy * std::cos(yaw) * dt; + + // (TODO) covariance prediction + + return output; +} + +/** + * @brief predict past or future tracked objects + * + * @param obj + * @param header + * @return TrackedObjects + */ +TrackedObjects predictPastOrFutureTrackedObjects( + const TrackedObjects & obj, const std_msgs::msg::Header & header) +{ + // for each object, predict past or future + TrackedObjects output_objects; + output_objects.header = obj.header; + output_objects.header.stamp = header.stamp; + + const auto dt = (rclcpp::Time(header.stamp) - rclcpp::Time(obj.header.stamp)).seconds(); + for (const auto & obj : obj.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj, dt)); + } + return output_objects; +} + +/** + * @brief interpolate tracked objects + * + * @param objects1 + * @param objects2 + * @param header : header of output object + * @return TrackedObjects + */ +TrackedObjects interpolateTrackedObjects( + const TrackedObjects & objects1, const TrackedObjects & objects2, std_msgs::msg::Header header) +{ + // Assumption: time of output header is between objects1 and objects2 + // time of objects1 < header < objects2 + + // define output objects + TrackedObjects output_objects; + output_objects.header = header; + + // calc weight + const auto dt = + (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(objects2.header.stamp)).seconds(); + const auto dt1 = (rclcpp::Time(objects1.header.stamp) - rclcpp::Time(header.stamp)).seconds(); + const auto weight = dt1 / dt; + + // check if object number is not zero + if (objects1.objects.size() == 0 && objects2.objects.size() == 0) { + // if objects1 and objects2 are empty, return empty objects + return output_objects; + } else if (objects1.objects.size() == 0) { + // if objects1 is empty return objects2 + for (const auto & obj2 : objects2.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj2, dt1 - dt)); + } + return output_objects; + } else if (objects2.objects.size() == 0) { + // if objects2 is empty return objects1 + for (const auto & obj1 : objects1.objects) { + output_objects.objects.push_back(predictPastOrFutureTrackedObject(obj1, dt1)); + } + } else { + // if both objects1 and objects2 are not empty do nothing + } + + // map to handle selected objects + std::unordered_map selected_objects1; + // iterate with int + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + selected_objects1[i] = false; + } + // search for objects with int iterator + for (std::size_t i = 0; i < objects1.objects.size(); i++) { + for (std::size_t j = 0; j < objects2.objects.size(); j++) { + if (objects1.objects[i].object_id == objects2.objects[j].object_id) { + // if id is same, interpolate + const auto interp_objects = + linearInterpolationForTrackedObject(objects1.objects[i], objects2.objects[j], weight); + output_objects.objects.push_back(interp_objects); + selected_objects1[i] = true; + break; + } + } + if (selected_objects1[i] == false) { + // if obj1 is not selected, predict future + const auto pred_objects = predictPastOrFutureTrackedObject(objects1.objects[i], dt1); + output_objects.objects.push_back(pred_objects); + } + } + + return output_objects; +} + +} // namespace utils + +namespace merger_utils +{ + +double mean(const double a, const double b) +{ + return (a + b) / 2.0; +} + +// object kinematics merger +// currently only support velocity fusion +autoware_auto_perception_msgs::msg::TrackedObjectKinematics objectKinematicsVXMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::TrackedObjectKinematics output_kinematics; + // copy main object at first + output_kinematics = main_obj.kinematics; + if (policy == MergePolicy::SKIP) { + return output_kinematics; + } else if (policy == MergePolicy::OVERWRITE) { + output_kinematics.twist_with_covariance.twist.linear.x = + sub_obj.kinematics.twist_with_covariance.twist.linear.x; + return output_kinematics; + } else if (policy == MergePolicy::FUSION) { + const auto main_vx = main_obj.kinematics.twist_with_covariance.twist.linear.x; + const auto sub_vx = sub_obj.kinematics.twist_with_covariance.twist.linear.x; + // inverse weight + const auto main_vx_cov = main_obj.kinematics.twist_with_covariance.covariance[0]; + const auto sub_vx_cov = sub_obj.kinematics.twist_with_covariance.covariance[0]; + double main_vx_weight, sub_vx_weight; + if (main_vx_cov == 0.0) { + main_vx_weight = 1.0 * 1e6; + } else { + main_vx_weight = 1.0 / main_vx_cov; + } + if (sub_vx_cov == 0.0) { + sub_vx_weight = 1.0 * 1e6; + } else { + sub_vx_weight = 1.0 / sub_vx_cov; + } + // merge with covariance + output_kinematics.twist_with_covariance.twist.linear.x = + (main_vx * main_vx_weight + sub_vx * sub_vx_weight) / (main_vx_weight + sub_vx_weight); + output_kinematics.twist_with_covariance.covariance[0] = 1.0 / (main_vx_weight + sub_vx_weight); + return output_kinematics; + } else { + std::cerr << "unknown merge policy in objectKinematicsMerger function." << std::endl; + return output_kinematics; + } + return output_kinematics; +} + +// object classification merger +TrackedObject objectClassificationMerger( + const TrackedObject & main_obj, const TrackedObject & sub_obj, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_obj; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj; + } else if (policy == MergePolicy::FUSION) { + // merge two std::vector into one + TrackedObject dummy_obj; + dummy_obj.classification = main_obj.classification; + for (const auto & sub_class : sub_obj.classification) { + dummy_obj.classification.push_back(sub_class); + } + return dummy_obj; + } else { + std::cerr << "unknown merge policy in objectClassificationMerger function." << std::endl; + return main_obj; + } +} + +// probability merger +float probabilityMerger(const float main_prob, const float sub_prob, const MergePolicy policy) +{ + if (policy == MergePolicy::SKIP) { + return main_prob; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_prob; + } else if (policy == MergePolicy::FUSION) { + return static_cast(mean(main_prob, sub_prob)); + } else { + std::cerr << "unknown merge policy in probabilityMerger function." << std::endl; + return main_prob; + } +} + +// shape merger +autoware_auto_perception_msgs::msg::Shape shapeMerger( + const autoware_auto_perception_msgs::msg::Shape & main_obj_shape, + const autoware_auto_perception_msgs::msg::Shape & sub_obj_shape, const MergePolicy policy) +{ + autoware_auto_perception_msgs::msg::Shape output_shape; + // copy main object at first + output_shape = main_obj_shape; + + if (main_obj_shape.type != sub_obj_shape.type) { + // if shape type is different, return main object + return output_shape; + } + + if (policy == MergePolicy::SKIP) { + return output_shape; + } else if (policy == MergePolicy::OVERWRITE) { + return sub_obj_shape; + } else if (policy == MergePolicy::FUSION) { + // write down fusion method for each shape type + if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + // if shape type is bounding box, merge bounding box + output_shape.dimensions.x = mean(main_obj_shape.dimensions.x, sub_obj_shape.dimensions.x); + output_shape.dimensions.y = mean(main_obj_shape.dimensions.y, sub_obj_shape.dimensions.y); + output_shape.dimensions.z = mean(main_obj_shape.dimensions.z, sub_obj_shape.dimensions.z); + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + // if shape type is cylinder, merge cylinder + // (TODO) implement + return output_shape; + } else if (main_obj_shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + // if shape type is polygon, merge polygon + // (TODO) + return output_shape; + } else { + // when type is unknown, print warning and do nothing + std::cerr << "unknown shape type in shapeMerger function." << std::endl; + return output_shape; + } + } else { + std::cerr << "unknown merge policy in shapeMerger function." << std::endl; + return output_shape; + } +} + +void updateExceptVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + const auto vx_temp = main_obj.kinematics.twist_with_covariance.twist.linear.x; + main_obj = sub_obj; + main_obj.kinematics.twist_with_covariance.twist.linear.x = vx_temp; +} + +void updateOnlyObjectVelocity(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj.kinematics = objectKinematicsVXMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateOnlyClassification(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = objectClassificationMerger(main_obj, sub_obj, MergePolicy::OVERWRITE); +} + +void updateWholeTrackedObject(TrackedObject & main_obj, const TrackedObject & sub_obj) +{ + main_obj = sub_obj; +} + +} // namespace merger_utils From 7e0031e988e41ad1124f241d202802dc3b7f671a Mon Sep 17 00:00:00 2001 From: "Md. Muhaimin Rahman" Date: Fri, 13 Oct 2023 11:48:25 +0900 Subject: [PATCH 078/206] fix(euclidean cluster): update the broken link (#5292) Update the broken link The given link was broken. I have updated with the correct link. --- perception/euclidean_cluster/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/euclidean_cluster/README.md b/perception/euclidean_cluster/README.md index 6c8995a44c56c..b57204c84cc3b 100644 --- a/perception/euclidean_cluster/README.md +++ b/perception/euclidean_cluster/README.md @@ -10,7 +10,7 @@ This package has two clustering methods: `euclidean_cluster` and `voxel_grid_bas ### euclidean_cluster -`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/en/latest/cluster_extraction.html) for details. +`pcl::EuclideanClusterExtraction` is applied to points. See [official document](https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html) for details. ### voxel_grid_based_euclidean_cluster From bcfae36caf2fd0948202c0c4992ae356239265d0 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 13 Oct 2023 13:07:24 +0900 Subject: [PATCH 079/206] docs(behavior_path_planner): update doc of safety check feature fot start planner (#5240) * update doc Signed-off-by: kyoichi-sugahara * rotate foot print Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- ...avior_path_planner_start_planner_design.md | 122 ++++++++++++++++-- .../collision_check_range.drawio.svg | 119 +++++++++++++++++ 2 files changed, 229 insertions(+), 12 deletions(-) create mode 100644 planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 95a9697a1d471..131202038c270 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -62,7 +62,7 @@ PullOutPath --o PullOutPlannerBase | Name | Unit | Type | Description | Default value | | :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | | th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | 0.1 | | th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | | th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | | th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | @@ -82,8 +82,108 @@ PullOutPath --o PullOutPlannerBase ## Safety check with dynamic obstacles +### **Basic concept of safety check against dynamic obstacles** + +This is based on the concept of RSS. For the logic used, refer to the link below. +See [safety check feature explanation](../docs/behavior_path_planner_safety_check.md) + +### **Collision check performed range** + +A collision check with dynamic objects is primarily performed between the shift start point and end point. The range for safety check varies depending on the type of path generated, so it will be explained for each pattern. + +#### **Shift pull out** + +For the "shift pull out", safety verification starts at the beginning of the shift and ends at the shift's conclusion. + +#### **Geometric pull out** + +Since there's a stop at the midpoint during the shift, this becomes the endpoint for safety verification. After stopping, safety verification resumes. + +#### **Backward pull out start point search** + +During backward movement, no safety check is performed. Safety check begins at the point where the backward movement ends. + +![collision_check_range](../image/start_planner/collision_check_range.drawio.svg) + +### **Ego vehicle's velocity planning** + +WIP + +### **Safety check in free space area** + WIP +## Parameters for safety check + +### Stop Condition Parameters + +Parameters under `stop_condition` define the criteria for stopping conditions. + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :------ | :----- | :-------------------------------------- | :------------ | +| maximum_deceleration_for_stop | [m/s^2] | double | Maximum deceleration allowed for a stop | 1.0 | +| maximum_jerk_for_stop | [m/s^3] | double | Maximum jerk allowed for a stop | 1.0 | + +### Ego Predicted Path Parameters + +Parameters under `path_safety_check.ego_predicted_path` specify the ego vehicle's predicted path characteristics. + +| Name | Unit | Type | Description | Default value | +| :---------------------------- | :------ | :----- | :--------------------------------------------------- | :------------ | +| min_velocity | [m/s] | double | Minimum velocity of the ego vehicle's predicted path | 0.0 | +| acceleration | [m/s^2] | double | Acceleration for the ego vehicle's predicted path | 1.0 | +| max_velocity | [m/s] | double | Maximum velocity of the ego vehicle's predicted path | 1.0 | +| time_horizon_for_front_object | [s] | double | Time horizon for predicting front objects | 10.0 | +| time_horizon_for_rear_object | [s] | double | Time horizon for predicting rear objects | 10.0 | +| time_resolution | [s] | double | Time resolution for the ego vehicle's predicted path | 0.5 | +| delay_until_departure | [s] | double | Delay until the ego vehicle departs | 1.0 | + +### Target Object Filtering Parameters + +Parameters under `target_filtering` are related to filtering target objects for safety check. + +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :---- | :----- | :------------------------------------------------- | :------------ | +| safety_check_time_horizon | [s] | double | Time horizon for safety check | 5.0 | +| safety_check_time_resolution | [s] | double | Time resolution for safety check | 1.0 | +| object_check_forward_distance | [m] | double | Forward distance for object detection | 10.0 | +| object_check_backward_distance | [m] | double | Backward distance for object detection | 100.0 | +| ignore_object_velocity_threshold | [m/s] | double | Velocity threshold below which objects are ignored | 1.0 | +| object_types_to_check.check_car | - | bool | Flag to check cars | true | +| object_types_to_check.check_truck | - | bool | Flag to check trucks | true | +| object_types_to_check.check_bus | - | bool | Flag to check buses | true | +| object_types_to_check.check_trailer | - | bool | Flag to check trailers | true | +| object_types_to_check.check_bicycle | - | bool | Flag to check bicycles | true | +| object_types_to_check.check_motorcycle | - | bool | Flag to check motorcycles | true | +| object_types_to_check.check_pedestrian | - | bool | Flag to check pedestrians | true | +| object_types_to_check.check_unknown | - | bool | Flag to check unknown object types | false | +| object_lane_configuration.check_current_lane | - | bool | Flag to check the current lane | true | +| object_lane_configuration.check_right_side_lane | - | bool | Flag to check the right side lane | true | +| object_lane_configuration.check_left_side_lane | - | bool | Flag to check the left side lane | true | +| object_lane_configuration.check_shoulder_lane | - | bool | Flag to check the shoulder lane | true | +| object_lane_configuration.check_other_lane | - | bool | Flag to check other lanes | false | +| include_opposite_lane | - | bool | Flag to include the opposite lane in check | false | +| invert_opposite_lane | - | bool | Flag to invert the opposite lane check | false | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| use_all_predicted_path | - | bool | Flag to use all predicted paths | true | +| use_predicted_path_outside_lanelet | - | bool | Flag to use predicted paths outside of lanelets | false | + +### Safety Check Parameters + +Parameters under `safety_check_params` define the configuration for safety check. + +| Name | Unit | Type | Description | Default value | +| :--------------------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | +| enable_safety_check | - | bool | Flag to enable safety check | true | +| check_all_predicted_path | - | bool | Flag to check all predicted paths | true | +| publish_debug_marker | - | bool | Flag to publish debug markers | false | +| rss_params.rear_vehicle_reaction_time | [s] | double | Reaction time for rear vehicles | 2.0 | +| rss_params.rear_vehicle_safety_time_margin | [s] | double | Safety time margin for rear vehicles | 1.0 | +| rss_params.lateral_distance_max_threshold | [m] | double | Maximum lateral distance threshold | 2.0 | +| rss_params.longitudinal_distance_min_threshold | [m] | double | Minimum longitudinal distance threshold | 3.0 | +| rss_params.longitudinal_velocity_delta_time | [s] | double | Delta time for longitudinal velocity | 0.8 | +| hysteresis_factor_expand_rate | - | double | Rate to expand/shrink the hysteresis factor | 1.0 | + ## **Path Generation** There are two path generation methods. @@ -113,9 +213,7 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral | 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 | +| 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** @@ -128,14 +226,14 @@ See also [[1]](https://www.sciencedirect.com/science/article/pii/S14746670153474 #### parameters for geometric pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------- | :---- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | -| divide_pull_out_path | [-] | bool | flag whether to divide arc paths.　The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | -| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | -| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | -| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | -| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | +| Name | Unit | Type | Description | Default value | +| :-------------------------- | :---- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_geometric_pull_out | [-] | bool | flag whether to enable geometric pull out | true | +| divide_pull_out_path | [-] | bool | flag whether to divide arc paths. The path is assumed to be divided because the curvature is not continuous. But it requires a stop during the departure. | false | +| geometric_pull_out_velocity | [m/s] | double | velocity of geometric pull out | 1.0 | +| arc_path_interval | [m] | double | path points interval of arc paths of geometric pull out | 1.0 | +| lane_departure_margin | [m] | double | margin of deviation to lane right | 0.2 | +| pull_out_max_steer_angle | [rad] | double | maximum steer angle for path generation | 0.26 | ## **backward pull out start point search** diff --git a/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg b/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg new file mode 100644 index 0000000000000..064f3602d6b6a --- /dev/null +++ b/planning/behavior_path_planner/image/start_planner/collision_check_range.drawio.svg @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
safety check range
+
+ + +
+
no-safety check range
+
+ + +
+
safety check start point with vehicle frame
+
+ + +
+
safety check end point with vehicle frame
+
+ + From 21acc282c523ce05c36fb59c70cb5b4a076e5ae8 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Fri, 13 Oct 2023 13:27:22 +0900 Subject: [PATCH 080/206] feat(ndt_scan_matcher): implement tpe sampler in ndt_align_srv (#5078) * Added output_pose_with_cov_to_log Signed-off-by: Shintaro Sakoda * Added logging particles Signed-off-by: Shintaro Sakoda * Added TreeStructuredParzenEstimator Signed-off-by: Shintaro Sakoda * FIxed to run TPE Signed-off-by: Shintaro Sakoda * Fixed initial pose Signed-off-by: Shintaro Sakoda * Fixed Input type Signed-off-by: Shintaro Sakoda * Fixed good condition Signed-off-by: Shintaro Sakoda * Fixed operation if good_num_ == 0 Signed-off-by: Shintaro Sakoda * Fixed cov Signed-off-by: Shintaro Sakoda * Fixed parameter Signed-off-by: Shintaro Sakoda * Fixed initial Signed-off-by: Shintaro Sakoda * Fixed to use good_threshold_ Signed-off-by: Shintaro Sakoda * Fixed to count good_num_ Signed-off-by: Shintaro Sakoda * Fixed stddev Signed-off-by: Shintaro Sakoda * Fixed parameter Signed-off-by: Shintaro Sakoda * Fixed parameters Signed-off-by: Shintaro Sakoda * Removed unused functions Signed-off-by: Shintaro Sakoda * Fixed to abs Signed-off-by: Shintaro Sakoda * Fixed parameters Signed-off-by: Shintaro Sakoda * FIxed parameter Signed-off-by: Shintaro Sakoda * add_trial by result Signed-off-by: Shintaro Sakoda * FIxed constructor Signed-off-by: Shintaro Sakoda * FIxed to use n_startup_trials_ Signed-off-by: Shintaro Sakoda * Fixed parameters Signed-off-by: Shintaro Sakoda * Added kTargetScore Signed-off-by: Shintaro Sakoda * Fixed constants Signed-off-by: Shintaro Sakoda * Removed output_pose_with_cov_to_log Signed-off-by: Shintaro Sakoda * Fixed a comment Signed-off-by: Shintaro Sakoda * Added a parameter "n_startup_trials" Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed include guard Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed hyphen to underscore Signed-off-by: Shintaro Sakoda * Added sample_from_prior Signed-off-by: Shintaro Sakoda * Fixed to use log_sum_exp Signed-off-by: Shintaro Sakoda * Fixed acquisition_function Signed-off-by: Shintaro Sakoda * Fixed n_startup_trials_ Signed-off-by: Shintaro Sakoda * Fixed to 99% Signed-off-by: Shintaro Sakoda * Fixed n_startup_trials_ Signed-off-by: Shintaro Sakoda * Fixed value of n_startup_trials Signed-off-by: Shintaro Sakoda * Removed log output Signed-off-by: Shintaro Sakoda * Fixed default value Signed-off-by: Shintaro Sakoda * Removed static distributions Signed-off-by: Shintaro Sakoda * Fixed PRIOR_WEIGHT Signed-off-by: Shintaro Sakoda * Fixed order Signed-off-by: Shintaro Sakoda * Added operator* to Input Signed-off-by: Shintaro Sakoda * Fixed parameters Signed-off-by: Shintaro Sakoda * Implemented normal tpe Signed-off-by: Shintaro Sakoda * Added is_loop_variable Signed-off-by: Shintaro Sakoda * Organized PRIOR_WEIGHT Signed-off-by: Shintaro Sakoda * Fixed PRIOR_WEIGHT 0.0 Signed-off-by: Shintaro Sakoda * Added CONVERT_COEFF Signed-off-by: Shintaro Sakoda * Fixed n_startup_trials Signed-off-by: Shintaro Sakoda * Renamed acquisition_function to compute_log_likelihood_ratio Signed-off-by: Shintaro Sakoda * Changed random number generator to static variables Signed-off-by: Shintaro Sakoda * Added reference Signed-off-by: Shintaro Sakoda * Fixed base_stddev_ to a const variable Signed-off-by: Shintaro Sakoda * Fixed to use VALUE_WIDTH Signed-off-by: Shintaro Sakoda * Removed a comment Signed-off-by: Shintaro Sakoda * Added `n_startup_trials` to README.md Signed-off-by: Shintaro Sakoda * Added a test about tpe Signed-off-by: Shintaro Sakoda * Added the license statement Signed-off-by: Shintaro Sakoda * Changed default `n_startup_trials` to 20 Signed-off-by: Shintaro Sakoda * Fixed sqrt(2) and added comments Signed-off-by: Shintaro SAKODA * Added comments Signed-off-by: Shintaro SAKODA * Added comments Signed-off-by: Shintaro SAKODA --------- Signed-off-by: Shintaro Sakoda Signed-off-by: Shintaro SAKODA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/ndt_scan_matcher/CMakeLists.txt | 11 + localization/ndt_scan_matcher/README.md | 1 + .../config/ndt_scan_matcher.param.yaml | 5 + .../ndt_scan_matcher_core.hpp | 1 + .../tree_structured_parzen_estimator.hpp | 80 +++++++ .../src/ndt_scan_matcher_core.cpp | 84 ++++++- .../src/tree_structured_parzen_estimator.cpp | 217 ++++++++++++++++++ .../ndt_scan_matcher/test/test_tpe.cpp | 65 ++++++ 8 files changed, 459 insertions(+), 5 deletions(-) create mode 100644 localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp create mode 100644 localization/ndt_scan_matcher/src/tree_structured_parzen_estimator.cpp create mode 100644 localization/ndt_scan_matcher/test/test_tpe.cpp diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 1d5a9d5ac5320..6f51b5210d853 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -34,11 +34,22 @@ ament_auto_add_executable(ndt_scan_matcher src/tf2_listener_module.cpp src/map_module.cpp src/map_update_module.cpp + src/tree_structured_parzen_estimator.cpp ) link_directories(${PCL_LIBRARY_DIRS}) target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_tpe + test/test_tpe.cpp + src/tree_structured_parzen_estimator.cpp + ) + target_include_directories(test_tpe PRIVATE include) + target_link_libraries(test_tpe) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index a1b9339b9780a..1f7779890f89a 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -68,6 +68,7 @@ One optional function is regularization. Please see the regularization chapter i | `converged_param_transform_probability` | double | TP threshold for deciding whether to trust the estimation result (when converged_param_type = 0) | | `converged_param_nearest_voxel_transformation_likelihood` | double | NVTL threshold for deciding whether to trust the estimation result (when converged_param_type = 1) | | `initial_estimate_particles_num` | int | The number of particles to estimate initial pose | +| `n_startup_trials` | int | The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). | | `lidar_topic_timeout_sec` | double | Tolerance of timestamp difference between current time and sensor pointcloud | | `initial_pose_timeout_sec` | int | Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] | | `initial_pose_distance_tolerance_m` | double | Tolerance of distance difference between two initial poses used for linear interpolation. [m] | diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index cf41a4cf55d0b..0ba1b1a2e15f4 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -43,6 +43,11 @@ # The number of particles to estimate initial pose initial_estimate_particles_num: 200 + # The number of initial random trials in the TPE (Tree-Structured Parzen Estimator). + # This value should be equal to or less than 'initial_estimate_particles_num' and more than 0. + # If it is equal to 'initial_estimate_particles_num', the search will be the same as a full random search. + n_startup_trials: 20 + # Tolerance of timestamp difference between current time and sensor pointcloud. [sec] lidar_topic_timeout_sec: 1.0 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 011cda6ba3356..fc4015aede059 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -179,6 +179,7 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_nearest_voxel_transformation_likelihood_; int initial_estimate_particles_num_; + int n_startup_trials_; double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp new file mode 100644 index 0000000000000..3c79ab75dba48 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp @@ -0,0 +1,80 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NDT_SCAN_MATCHER__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define NDT_SCAN_MATCHER__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ + +/* +A implementation of tree-structured parzen estimator (TPE) +See below pdf for the TPE algorithm detail. +https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf + +Optuna is also used as a reference for implementation. +https://github.com/optuna/optuna +*/ + +#include +#include +#include + +class TreeStructuredParzenEstimator +{ +public: + using Input = std::vector; + using Score = double; + struct Trial + { + Input input; + Score score; + }; + + enum Direction { + MINIMIZE = 0, + MAXIMIZE = 1, + }; + + TreeStructuredParzenEstimator() = delete; + TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector is_loop_variable); + void add_trial(const Trial & trial); + Input get_next_input() const; + +private: + static constexpr double BASE_STDDEV_COEFF = 0.2; + static constexpr double MAX_GOOD_RATE = 0.10; + static constexpr double MAX_VALUE = 1.0; + static constexpr double MIN_VALUE = -1.0; + static constexpr double VALUE_WIDTH = MAX_VALUE - MIN_VALUE; + static constexpr int64_t N_EI_CANDIDATES = 100; + static constexpr double PRIOR_WEIGHT = 0.0; + + static std::mt19937_64 engine; + static std::uniform_real_distribution dist_uniform; + static std::normal_distribution dist_normal; + + double compute_log_likelihood_ratio(const Input & input) const; + double log_gaussian_pdf(const Input & input, const Input & mu, const Input & sigma) const; + static std::vector get_weights(const int64_t n); + static double normalize_loop_variable(const double value); + + std::vector trials_; + int64_t above_num_; + const Direction direction_; + const int64_t n_startup_trials_; + const int64_t input_dimension_; + const std::vector is_loop_variable_; + const Input base_stddev_; +}; + +#endif // NDT_SCAN_MATCHER__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index d9054d77d2256..238e5c6be89bc 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -17,11 +17,14 @@ #include "ndt_scan_matcher/matrix_type.hpp" #include "ndt_scan_matcher/particle.hpp" #include "ndt_scan_matcher/pose_array_interpolator.hpp" +#include "ndt_scan_matcher/tree_structured_parzen_estimator.hpp" #include "ndt_scan_matcher/util_func.hpp" #include #include +#include + #include #ifdef ROS_DISTRO_GALACTIC @@ -145,6 +148,7 @@ NDTScanMatcher::NDTScanMatcher() } initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); + n_startup_trials_ = this->declare_parameter("n_startup_trials"); estimate_scores_for_degrounded_scan_ = this->declare_parameter("estimate_scores_for_degrounded_scan"); @@ -784,15 +788,64 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ output_pose_with_cov_to_log(get_logger(), "align_using_monte_carlo_input", initial_pose_with_cov); - // generateParticle - const auto initial_poses = - create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); + const auto base_rpy = get_rpy(initial_pose_with_cov); + const Eigen::Map covariance = { + initial_pose_with_cov.pose.covariance.data(), 6, 6}; + const double stddev_x = std::sqrt(covariance(0, 0)); + const double stddev_y = std::sqrt(covariance(1, 1)); + const double stddev_z = std::sqrt(covariance(2, 2)); + const double stddev_roll = std::sqrt(covariance(3, 3)); + const double stddev_pitch = std::sqrt(covariance(4, 4)); + + // Let phi be the cumulative distribution function of the standard normal distribution. + // It has the following relationship with the error function (erf). + // phi(x) = 1/2 (1 + erf(x / sqrt(2))) + // so, 2 * phi(x) - 1 = erf(x / sqrt(2)). + // The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in + // TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good + // to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps). + const double SQRT2 = std::sqrt(2); + auto uniform_to_normal = [&SQRT2](const double uniform) { + assert(-1.0 <= uniform && uniform <= 1.0); + constexpr double epsilon = 1.0e-6; + const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon); + return boost::math::erf_inv(clamped) * SQRT2; + }; + + auto normal_to_uniform = [&SQRT2](const double normal) { + return boost::math::erf(normal / SQRT2); + }; + + // Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions. + // The last dimension (yaw) is a loop variable. + // Although roll and pitch are also angles, they are considered non-looping variables that follow + // a normal distribution with a small standard deviation. This assumes that the initial pose of + // the ego vehicle is aligned with the ground to some extent about roll and pitch. + const std::vector is_loop_variable = {false, false, false, false, false, true}; + TreeStructuredParzenEstimator tpe( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials_, is_loop_variable); std::vector particle_array; auto output_cloud = std::make_shared>(); - for (unsigned int i = 0; i < initial_poses.size(); i++) { - const auto & initial_pose = initial_poses[i]; + for (int i = 0; i < initial_estimate_particles_num_; i++) { + const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); + + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = + initial_pose_with_cov.pose.pose.position.x + uniform_to_normal(input[0]) * stddev_x; + initial_pose.position.y = + initial_pose_with_cov.pose.pose.position.y + uniform_to_normal(input[1]) * stddev_y; + initial_pose.position.z = + initial_pose_with_cov.pose.pose.position.z + uniform_to_normal(input[2]) * stddev_z; + geometry_msgs::msg::Vector3 init_rpy; + init_rpy.x = base_rpy.x + uniform_to_normal(input[3]) * stddev_roll; + init_rpy.y = base_rpy.y + uniform_to_normal(input[4]) * stddev_pitch; + init_rpy.z = base_rpy.z + input[5] * M_PI; + tf2::Quaternion tf_quaternion; + tf_quaternion.setRPY(init_rpy.x, init_rpy.y, init_rpy.z); + initial_pose.orientation = tf2::toMsg(tf_quaternion); + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); ndt_ptr->align(*output_cloud, initial_pose_matrix); const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); @@ -806,6 +859,27 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ particle, i); ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose); + const geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + + const double diff_x = pose.position.x - initial_pose_with_cov.pose.pose.position.x; + const double diff_y = pose.position.y - initial_pose_with_cov.pose.pose.position.y; + const double diff_z = pose.position.z - initial_pose_with_cov.pose.pose.position.z; + const double diff_roll = rpy.x - base_rpy.x; + const double diff_pitch = rpy.y - base_rpy.y; + const double diff_yaw = rpy.z - base_rpy.z; + + // Only yaw is a loop_variable, so only simple normalization is performed. + // All other variables are converted from normal distribution to uniform distribution. + TreeStructuredParzenEstimator::Input result(is_loop_variable.size()); + result[0] = normal_to_uniform(diff_x / stddev_x); + result[1] = normal_to_uniform(diff_y / stddev_y); + result[2] = normal_to_uniform(diff_z / stddev_z); + result[3] = normal_to_uniform(diff_roll / stddev_roll); + result[4] = normal_to_uniform(diff_pitch / stddev_pitch); + result[5] = diff_yaw / M_PI; + tpe.add_trial(TreeStructuredParzenEstimator::Trial{result, ndt_result.transform_probability}); + auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( *ndt_ptr->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); diff --git a/localization/ndt_scan_matcher/src/tree_structured_parzen_estimator.cpp b/localization/ndt_scan_matcher/src/tree_structured_parzen_estimator.cpp new file mode 100644 index 0000000000000..b92c8a075252c --- /dev/null +++ b/localization/ndt_scan_matcher/src/tree_structured_parzen_estimator.cpp @@ -0,0 +1,217 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ndt_scan_matcher/tree_structured_parzen_estimator.hpp" + +#include +#include +#include +#include + +// random number generator +std::mt19937_64 TreeStructuredParzenEstimator::engine(std::random_device{}()); +std::uniform_real_distribution TreeStructuredParzenEstimator::dist_uniform( + TreeStructuredParzenEstimator::MIN_VALUE, TreeStructuredParzenEstimator::MAX_VALUE); +std::normal_distribution TreeStructuredParzenEstimator::dist_normal(0.0, 1.0); + +TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( + const Direction direction, const int64_t n_startup_trials, std::vector is_loop_variable) +: above_num_(0), + direction_(direction), + n_startup_trials_(n_startup_trials), + input_dimension_(is_loop_variable.size()), + is_loop_variable_(is_loop_variable), + base_stddev_(input_dimension_, VALUE_WIDTH) +{ +} + +void TreeStructuredParzenEstimator::add_trial(const Trial & trial) +{ + trials_.push_back(trial); + std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { + return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); + }); + above_num_ = + std::min(static_cast(25), static_cast(trials_.size() * MAX_GOOD_RATE)); +} + +TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const +{ + if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { + // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. + Input input(input_dimension_); + for (int64_t j = 0; j < input_dimension_; j++) { + input[j] = dist_uniform(engine); + } + return input; + } + + Input best_input; + double best_log_likelihood_ratio = std::numeric_limits::lowest(); + const double coeff = BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_)); + std::vector weights = get_weights(above_num_); + weights.push_back(PRIOR_WEIGHT); + std::discrete_distribution dist(weights.begin(), weights.end()); + for (int64_t i = 0; i < N_EI_CANDIDATES; i++) { + Input mu, sigma; + const int64_t index = dist(engine); + if (index == above_num_) { + mu = Input(input_dimension_, 0.0); + sigma = base_stddev_; + } else { + mu = trials_[index].input; + sigma = base_stddev_; + for (int64_t j = 0; j < input_dimension_; j++) { + sigma[j] *= coeff; + } + } + // sample from the normal distribution + Input input(input_dimension_); + for (int64_t j = 0; j < input_dimension_; j++) { + input[j] = mu[j] + dist_normal(engine) * sigma[j]; + input[j] = + (is_loop_variable_[j] ? normalize_loop_variable(input[j]) + : std::clamp(input[j], MIN_VALUE, MAX_VALUE)); + } + const double log_likelihood_ratio = compute_log_likelihood_ratio(input); + if (log_likelihood_ratio > best_log_likelihood_ratio) { + best_log_likelihood_ratio = log_likelihood_ratio; + best_input = input; + } + } + return best_input; +} + +double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const +{ + const int64_t n = trials_.size(); + + // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to + // select best sample. + std::vector above_logs; + std::vector below_logs; + + // Scott's rule + const double coeff_above = + BASE_STDDEV_COEFF * std::pow(above_num_, -1.0 / (4 + input_dimension_)); + const double coeff_below = + BASE_STDDEV_COEFF * std::pow(n - above_num_, -1.0 / (4 + input_dimension_)); + Input sigma_above = base_stddev_; + Input sigma_below = base_stddev_; + for (int64_t j = 0; j < input_dimension_; j++) { + sigma_above[j] *= coeff_above; + sigma_below[j] *= coeff_below; + } + + std::vector above_weights = get_weights(above_num_); + std::vector below_weights = get_weights(n - above_num_); + std::reverse(below_weights.begin(), below_weights.end()); // below_weights is ascending order + + // calculate the sum of weights to normalize + double above_sum = std::accumulate(above_weights.begin(), above_weights.end(), 0.0); + double below_sum = std::accumulate(below_weights.begin(), below_weights.end(), 0.0); + + // above includes prior + above_sum += PRIOR_WEIGHT; + + for (int64_t i = 0; i < n; i++) { + if (i < above_num_) { + const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_above); + const double w = above_weights[i] / above_sum; + const double log_w = std::log(w); + above_logs.push_back(log_p + log_w); + } else { + const double log_p = log_gaussian_pdf(input, trials_[i].input, sigma_below); + const double w = below_weights[i - above_num_] / below_sum; + const double log_w = std::log(w); + below_logs.push_back(log_p + log_w); + } + } + + // prior + if (PRIOR_WEIGHT > 0.0) { + const double log_p = log_gaussian_pdf(input, Input(input_dimension_, 0.0), base_stddev_); + const double log_w = std::log(PRIOR_WEIGHT / above_sum); + above_logs.push_back(log_p + log_w); + } + + auto log_sum_exp = [](const std::vector & log_vec) { + const double max = *std::max_element(log_vec.begin(), log_vec.end()); + double sum = 0.0; + for (const double log_v : log_vec) { + sum += std::exp(log_v - max); + } + return max + std::log(sum); + }; + + const double above = log_sum_exp(above_logs); + const double below = log_sum_exp(below_logs); + const double r = above - below; + return r; +} + +double TreeStructuredParzenEstimator::log_gaussian_pdf( + const Input & input, const Input & mu, const Input & sigma) const +{ + const double log_2pi = std::log(2.0 * M_PI); + auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { + return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); + }; + + const int64_t n = input.size(); + + double result = 0.0; + for (int64_t i = 0; i < n; i++) { + double diff = input[i] - mu[i]; + if (is_loop_variable_[i]) { + diff = normalize_loop_variable(diff); + } + result += log_gaussian_pdf_1d(diff, sigma[i]); + } + return result; +} + +std::vector TreeStructuredParzenEstimator::get_weights(const int64_t n) +{ + // See optuna + // https://github.com/optuna/optuna/blob/4bfab78e98bf786f6a2ce6e593a26e3f8403e08d/optuna/samplers/_tpe/sampler.py#L50-L58 + std::vector weights; + constexpr int64_t WEIGHT_ALPHA = 25; + if (n == 0) { + return weights; + } else if (n < WEIGHT_ALPHA) { + weights.resize(n, 1.0); + } else { + weights.resize(n); + const double unit = (1.0 - 1.0 / n) / (n - WEIGHT_ALPHA); + for (int64_t i = 0; i < n; i++) { + weights[i] = (i < WEIGHT_ALPHA ? 1.0 : 1.0 - unit * (i - WEIGHT_ALPHA)); + } + } + + return weights; +} + +double TreeStructuredParzenEstimator::normalize_loop_variable(const double value) +{ + // Normalize the loop variable to [-1, 1) + double result = value; + while (result >= MAX_VALUE) { + result -= VALUE_WIDTH; + } + while (result < MIN_VALUE) { + result += VALUE_WIDTH; + } + return result; +} diff --git a/localization/ndt_scan_matcher/test/test_tpe.cpp b/localization/ndt_scan_matcher/test/test_tpe.cpp new file mode 100644 index 0000000000000..fb7190f4a1c74 --- /dev/null +++ b/localization/ndt_scan_matcher/test/test_tpe.cpp @@ -0,0 +1,65 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ndt_scan_matcher/tree_structured_parzen_estimator.hpp" + +#include + +TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) +{ + auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { + double value = 0.0; + const int64_t n = input.size(); + for (int64_t i = 0; i < n; i++) { + const double v = input[i] * 10; + value += v * v; + } + return value; + }; + + constexpr int64_t kOuterTrialsNum = 10; + constexpr int64_t kInnerTrialsNum = 100; + std::cout << std::fixed; + std::vector mean_scores; + for (const int64_t n_startup_trials : {kInnerTrialsNum, kInnerTrialsNum / 10}) { + const std::string method = ((n_startup_trials == kInnerTrialsNum) ? "Random" : "TPE"); + + std::vector scores; + for (int64_t i = 0; i < kOuterTrialsNum; i++) { + double best_score = std::numeric_limits::lowest(); + const std::vector is_loop_variable(6, false); + TreeStructuredParzenEstimator estimator( + TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, is_loop_variable); + for (int64_t trial = 0; trial < kInnerTrialsNum; trial++) { + const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); + const double score = -sphere_function(input); + estimator.add_trial({input, score}); + best_score = std::max(best_score, score); + } + scores.push_back(best_score); + } + + const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); + const double mean = sum / scores.size(); + mean_scores.push_back(mean); + double sq_sum = 0.0; + for (const double score : scores) { + sq_sum += (score - mean) * (score - mean); + } + const double stddev = std::sqrt(sq_sum / scores.size()); + + std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; + } + ASSERT_LT(mean_scores[0], mean_scores[1]); +} From 69560ad3f1ccf7576329c75efdc5ec3d11d94d88 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 13 Oct 2023 13:39:32 +0900 Subject: [PATCH 081/206] fix(lane_change): fix force path not appear (#5288) Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 913ddcacf08bc..21fa1495fba75 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1222,12 +1222,12 @@ bool NormalLaneChange::getLaneChangePaths( logger_, "Stop time is over threshold. Allow lane change in intersection."); } + candidate_paths->push_back(*candidate_path); if (utils::lane_change::passParkedObject( route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, is_goal_in_route, *lane_change_parameters_)) { return false; } - candidate_paths->push_back(*candidate_path); if (!check_safety) { return false; From df836214c7443f4ad92e5abc9ee601c1cae4bf5b Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 13 Oct 2023 13:41:39 +0900 Subject: [PATCH 082/206] fix(lane_change): prevent node from dying for trimmed shifted path (#5271) Signed-off-by: Zulfaqar Azmi --- .../src/utils/lane_change/utils.cpp | 9 +++++++++ planning/behavior_path_planner/src/utils/path_utils.cpp | 2 +- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 038cf9488345f..a76b3cbbaf1e0 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -301,6 +301,15 @@ std::optional constructCandidatePath( "failed to generate shifted path."); } + // TODO(Zulfaqar Azmi): have to think of a more feasible solution for points being remove by path + // shifter. + if (shifted_path.path.points.size() < shift_line.end_idx + 1) { + RCLCPP_DEBUG( + rclcpp::get_logger("behavior_path_planner").get_child("utils").get_child(__func__), + "path points are removed by PathShifter."); + return std::nullopt; + } + const auto & prepare_length = lane_change_length.prepare; const auto & lane_changing_length = lane_change_length.lane_changing; diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 509b31ad3da2b..1ac63f4a4be87 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -226,7 +226,7 @@ std::pair getPathTurnSignal( turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; const double max_time = std::numeric_limits::max(); const double max_distance = std::numeric_limits::max(); - if (path.shift_length.empty()) { + if (path.shift_length.size() < shift_line.end_idx + 1) { return std::make_pair(turn_signal, max_distance); } const auto base_link2front = common_parameter.base_link2front; From f1cf22b1d44b20a33e7df1533bef5a8854e29562 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 13 Oct 2023 13:45:12 +0900 Subject: [PATCH 083/206] fix(lane_change): add visualization for passParkedObject (#5291) Signed-off-by: Muhammad Zulfaqar Azmi --- .../behavior_path_planner/utils/lane_change/utils.hpp | 4 +++- .../src/scene_module/lane_change/normal.cpp | 2 +- .../behavior_path_planner/src/utils/lane_change/utils.cpp | 6 +++++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index b2592bc1b899b..cd8f293b2e610 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -52,6 +52,7 @@ using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using path_safety_checker::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; @@ -170,7 +171,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters); + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug); boost::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 21fa1495fba75..375bd1e803d58 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1225,7 +1225,7 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->push_back(*candidate_path); if (utils::lane_change::passParkedObject( route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_)) { + is_goal_in_route, *lane_change_parameters_, object_debug_)) { return false; } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index a76b3cbbaf1e0..6d25cb8751a8e 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -939,7 +939,8 @@ bool isParkedObject( bool passParkedObject( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters) + const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, + CollisionCheckDebugMap & object_debug) { const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; @@ -962,6 +963,7 @@ bool passParkedObject( } const auto & leading_obj = objects.at(*leading_obj_idx); + auto debug = marker_utils::createObjectDebug(leading_obj); const auto leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { @@ -985,6 +987,8 @@ bool passParkedObject( // If there are still enough length after the target object, we delay the lane change if (min_dist_to_end_of_current_lane > minimum_lane_change_length) { + debug.second.unsafe_reason = "delay lane change"; + marker_utils::updateCollisionCheckDebugMap(object_debug, debug, false); return true; } From 61c7e3205466fd6bd80a8fa6432f268d65a788d2 Mon Sep 17 00:00:00 2001 From: Yamato Ando Date: Fri, 13 Oct 2023 14:11:55 +0900 Subject: [PATCH 084/206] refactor(map_packages): remove unused depend in pakcages.xml files (#5172) Signed-off-by: yamato-ando Co-authored-by: yamato-ando --- map/map_height_fitter/package.xml | 1 - map/map_loader/CMakeLists.txt | 2 +- map/map_loader/package.xml | 5 ----- map/map_projection_loader/package.xml | 1 - map/util/lanelet2_map_preprocessor/CMakeLists.txt | 6 ++++++ map/util/lanelet2_map_preprocessor/package.xml | 2 +- 6 files changed, 8 insertions(+), 9 deletions(-) diff --git a/map/map_height_fitter/package.xml b/map/map_height_fitter/package.xml index 8f6ccf969c9a4..f50eba9218d67 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -19,7 +19,6 @@ pcl_conversions rclcpp sensor_msgs - tf2 tf2_geometry_msgs tf2_ros tier4_localization_msgs diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 745edd0daca99..b94eaac7ee34d 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -4,7 +4,7 @@ project(map_loader) find_package(autoware_cmake REQUIRED) autoware_package() -find_package(PCL REQUIRED COMPONENTS io) +find_package(PCL REQUIRED COMPONENTS common io filters) ament_auto_add_library(pointcloud_map_loader_node SHARED src/pointcloud_map_loader/pointcloud_map_loader_node.cpp diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index e663d19d516ac..5230d4bd03214 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -24,13 +24,8 @@ lanelet2_extension libpcl-all-dev pcl_conversions - pcl_ros rclcpp rclcpp_components - std_msgs - tf2_geometry_msgs - tf2_ros - tier4_autoware_utils tier4_map_msgs visualization_msgs yaml-cpp diff --git a/map/map_projection_loader/package.xml b/map/map_projection_loader/package.xml index 03053533d3ead..b128c2cf15e15 100644 --- a/map/map_projection_loader/package.xml +++ b/map/map_projection_loader/package.xml @@ -16,7 +16,6 @@ component_interface_utils lanelet2_extension rclcpp - rclcpp_components tier4_map_msgs yaml-cpp diff --git a/map/util/lanelet2_map_preprocessor/CMakeLists.txt b/map/util/lanelet2_map_preprocessor/CMakeLists.txt index caa1d27b3e292..8a6b16c05011b 100644 --- a/map/util/lanelet2_map_preprocessor/CMakeLists.txt +++ b/map/util/lanelet2_map_preprocessor/CMakeLists.txt @@ -4,12 +4,18 @@ project(lanelet2_map_preprocessor) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(PCL REQUIRED COMPONENTS common io kdtree) + include_directories( include SYSTEM ${PCL_INCLUDE_DIRS} ) +link_libraries( + ${PCL_LIBRARIES} +) + ament_auto_add_executable(fix_z_value_by_pcd src/fix_z_value_by_pcd.cpp) ament_auto_add_executable(transform_maps src/transform_maps.cpp) ament_auto_add_executable(merge_close_lines src/merge_close_lines.cpp) diff --git a/map/util/lanelet2_map_preprocessor/package.xml b/map/util/lanelet2_map_preprocessor/package.xml index 15f061369e649..79fce94f87a95 100644 --- a/map/util/lanelet2_map_preprocessor/package.xml +++ b/map/util/lanelet2_map_preprocessor/package.xml @@ -11,7 +11,7 @@ autoware_cmake lanelet2_extension - pcl_ros + libpcl-all-dev rclcpp ament_lint_auto From 910f30bcd776b48b9c7dadcc3b638236aa4006ab Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 13 Oct 2023 15:43:51 +0900 Subject: [PATCH 085/206] fix(behavior_path_planner): fix calcMinimumLongitudinalLength args (#5299) Signed-off-by: kosuke55 --- .../utils/path_safety_checker/safety_check.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 4983060aa1c0a..6e5dd2cda8920 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -80,7 +80,7 @@ double calcRssDistance( double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, - const BehaviorPathPlannerParameters & params); + const RSSparams & rss_params); boost::optional calcInterpolatedPoseWithVelocity( const std::vector & path, const double relative_time); From d9abd595eecd73796ba4fd481c9f395760c187be Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 13 Oct 2023 19:28:50 +0900 Subject: [PATCH 086/206] fix(lane_change): update target lane obj block condition (#5296) fix(lane_change): target lane blocking condition remove in_terminal condition fix Signed-off-by: Takamasa Horibe --- .../src/scene_module/lane_change/normal.cpp | 122 ++++++++++-------- 1 file changed, 68 insertions(+), 54 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 375bd1e803d58..c332438dd31a5 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -194,79 +194,93 @@ void NormalLaneChange::insertStopPoint( const double lane_change_buffer = utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { + return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); + }; + // If lanelets.back() is in goal route section, get distance to goal. // Otherwise, get distance to end of lane. double distance_to_terminal = 0.0; if (route_handler->isInGoalRouteSection(lanelets.back())) { const auto goal = route_handler->getGoalPose(); - distance_to_terminal = utils::getSignedDistance(path.points.front().point.pose, goal, lanelets); + distance_to_terminal = getDistanceAlongLanelet(goal); } else { distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); } const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); - const bool is_in_terminal_section = lanelet::utils::isInLanelet(getEgoPose(), lanelets.back()) || - distance_to_terminal < lane_change_buffer; - const bool has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { - if (o.initial_twist.twist.linear.x > lane_change_parameters_->stop_velocity_threshold) { - return false; + double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + + // calculate minimum distance from path front to the stationary object on the ego lane. + const auto distance_to_ego_lane_obj = [&]() -> double { + double distance_to_obj = distance_to_terminal; + const double distance_to_ego = getDistanceAlongLanelet(getEgoPose()); + + for (const auto & object : target_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + continue; } - const double distance_to_target_lane_obj = utils::getSignedDistance( - path.points.front().point.pose, o.initial_pose.pose, status_.target_lanes); - return distance_to_target_lane_obj < distance_to_terminal; - }); - // auto stopping_distance = raw_stopping_distance; - double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; - if (is_in_terminal_section || !has_blocking_target_lane_obj) { - // calculate minimum distance from path front to the stationary object on the ego lane. - const auto distance_to_ego_lane_obj = [&]() -> double { - double distance_to_obj = distance_to_terminal; - const double distance_to_ego = - utils::getSignedDistance(path.points.front().point.pose, getEgoPose(), lanelets); - - for (const auto & object : target_objects.current_lane) { - // check if stationary - const auto obj_v = std::abs(object.initial_twist.twist.linear.x); - if (obj_v > lane_change_parameters_->stop_velocity_threshold) { + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto polygon = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : polygon) { + const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + + // ignore if the point is around the ego path + if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { continue; } - // calculate distance from path front to the stationary object polygon on the ego lane. - const auto polygon = - tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); - for (const auto & polygon_p : polygon) { - const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp); + const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } - - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); - - // ignore backward object - if (current_distance_to_obj < distance_to_ego) { - continue; - } - distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); + // ignore backward object + if (current_distance_to_obj < distance_to_ego) { + continue; } + distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); } - return distance_to_obj; - }(); - - // If the lane change space is occupied by a stationary obstacle, move the stop line closer. - // TODO(Horibe): We need to loop this process because the new space could be occupied as well. - stopping_distance = std::min( - distance_to_ego_lane_obj - lane_change_buffer - stop_point_buffer - - getCommonParam().base_link2front - - calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, - lane_change_parameters_->rss_params), - stopping_distance); + } + return distance_to_obj; + }(); + + // Need to stop before blocking obstacle + if (distance_to_ego_lane_obj < distance_to_terminal) { + // consider rss distance when the LC need to avoid obstacles + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer - + stop_point_buffer - rss_dist - + getCommonParam().base_link2front; + + // If the target lane in the lane change section is blocked by a stationary obstacle, there + // is no reason for stopping with a lane change margin. Instead, stop right behind the + // obstacle. + // ---------------------------------------------------------- + // [obj]> + // ---------------------------------------------------------- + // [ego]> | <--- lane change margin ---> [obj]> + // ---------------------------------------------------------- + const bool has_blocking_target_lane_obj = std::any_of( + target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) { + const auto v = std::abs(o.initial_twist.twist.linear.x); + if (v > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + return stopping_distance_for_obj < distance_to_target_lane_obj && + distance_to_target_lane_obj < distance_to_ego_lane_obj; + }); + + if (!has_blocking_target_lane_obj) { + stopping_distance = stopping_distance_for_obj; + } } if (stopping_distance > 0.0) { From c03b5ac3abde3e4240e6c36d77c8bc939df90ea8 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 13 Oct 2023 19:47:44 +0900 Subject: [PATCH 087/206] fix(tier4_autoware_utils): fix `-Werror=float-conversion` (#5301) fix(tier4_autoware_utils): fix Signed-off-by: satoshi-ota --- .../include/tier4_autoware_utils/geometry/geometry.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 17ce7e1f4de5a..6106e3a15f4c7 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -261,7 +261,7 @@ inline void setOrientation(const geometry_msgs::msg::Quaternion & orientation, T } template -void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unused]] T & p) +void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unused]] T & p) { static_assert(sizeof(T) == 0, "Only specializations of getLongitudinalVelocity can be used."); throw std::logic_error("Only specializations of getLongitudinalVelocity can be used."); @@ -269,21 +269,21 @@ void setLongitudinalVelocity([[maybe_unused]] const double velocity, [[maybe_unu template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::PathPoint & p) + const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } From def9ffc14a91a89e417f82d5ee6aafbc17944058 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 13 Oct 2023 20:24:30 +0900 Subject: [PATCH 088/206] feat(simple_planning_sim): publish lateral acceleration (#5307) Signed-off-by: Takamasa Horibe --- .../simple_planning_simulator_core.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 208fd80d57a08..fb6a5457e8f05 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -638,6 +638,7 @@ void SimplePlanningSimulator::publish_acceleration() msg.header.frame_id = "/base_link"; msg.header.stamp = get_clock()->now(); msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); + msg.accel.accel.linear.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; @@ -658,6 +659,7 @@ void SimplePlanningSimulator::publish_imu() imu.header.frame_id = "base_link"; imu.header.stamp = now(); imu.linear_acceleration.x = vehicle_model_ptr_->getAx(); + imu.linear_acceleration.y = vehicle_model_ptr_->getWz() * vehicle_model_ptr_->getVx(); constexpr auto COV = 0.001; imu.linear_acceleration_covariance.at(COV_IDX::X_X) = COV; imu.linear_acceleration_covariance.at(COV_IDX::Y_Y) = COV; From 8b1f3c0c58e78d6946a27773e51d1f36c9334581 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 13 Oct 2023 21:31:46 +0900 Subject: [PATCH 089/206] feat(intersection): new feature of yield stuck detection (#5270) * feat(intersection): new feature of yield stuck detection Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * use turn_direction Signed-off-by: Takayuki Murooka * update config Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka --- .../config/intersection.param.yaml | 12 ++- .../src/debug.cpp | 5 + .../src/manager.cpp | 8 ++ .../src/scene_intersection.cpp | 98 +++++++++++++++++++ .../src/scene_intersection.hpp | 16 ++- .../src/util.cpp | 39 ++++++++ .../src/util.hpp | 6 ++ .../src/util_type.hpp | 1 + 8 files changed, 181 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index bb80c140b95fb..30f4bc7b42a25 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -24,6 +24,12 @@ # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. + yield_stuck: + turn_direction: + left: true + right: false + straight: false + distance_thr: 1.0 # [m] collision_detection: state_transit_margin_time: 1.0 @@ -42,9 +48,9 @@ use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: - distance_to_assigned_lanelet_start: 5.0 - duration: 2.0 - range: 30.0 # [m] + distance_to_assigned_lanelet_start: 10.0 + duration: 3.0 + range: 50.0 # [m] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 9ff638cb61876..82f5b12966c12 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -238,6 +238,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.yield_stuck_targets, "stuck_targets", module_id_, now, 0.4, 0.99, 0.2), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.blocking_attention_objects, "blocking_attention_objects", module_id_, now, 0.99, diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3cca1f42115d1..93d67e786cff5 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -85,6 +85,14 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); ip.stuck_vehicle.enable_private_area_stuck_disregard = getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); + ip.stuck_vehicle.yield_stuck_turn_direction.left = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.left"); + ip.stuck_vehicle.yield_stuck_turn_direction.right = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.right"); + ip.stuck_vehicle.yield_stuck_turn_direction.straight = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.straight"); + ip.stuck_vehicle.yield_stuck_distance_thr = + getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.distance_thr"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index ee541a061ce0e..76c2dc072dda9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -195,6 +195,21 @@ void prepareRTCByDecisionResult( return; } +template <> +void prepareRTCByDecisionResult( + const IntersectionModule::YieldStuckStop & result, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) +{ + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); + const auto closest_idx = result.closest_idx; + const auto stop_line_idx = result.stuck_stop_line_idx; + *default_safety = false; + *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *occlusion_safety = true; + return; +} + template <> void prepareRTCByDecisionResult( const IntersectionModule::NonOccludedCollisionStop & result, @@ -419,6 +434,38 @@ void reactRTCApprovalByDecisionResult( return; } +template <> +void reactRTCApprovalByDecisionResult( + const bool rtc_default_approved, const bool rtc_occlusion_approved, + const IntersectionModule::YieldStuckStop & decision_result, + [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, + const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) +{ + RCLCPP_DEBUG( + rclcpp::get_logger("reactRTCApprovalByDecisionResult"), + "YieldStuckStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, + rtc_occlusion_approved); + const auto closest_idx = decision_result.closest_idx; + if (!rtc_default_approved) { + // use default_rtc uuid for stuck vehicle detection + const auto stop_line_idx = decision_result.stuck_stop_line_idx; + planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + debug_data->collision_stop_wall_pose = + planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + { + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); + planning_utils::appendStopReason(stop_factor, stop_reason); + velocity_factor->set( + path->points, path->points.at(closest_idx).point.pose, + path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + } + } + return; +} + template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, @@ -780,6 +827,9 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult if (std::holds_alternative(decision_result)) { return "StuckStop"; } + if (std::holds_alternative(decision_result)) { + return "YieldStuckStop"; + } if (std::holds_alternative(decision_result)) { return "NonOccludedCollisionStop"; } @@ -985,6 +1035,23 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } } + // yield stuck vehicle detection is viable even if attention area is empty + // so this needs to be checked before attention area validation + const bool yield_stuck_detected = checkYieldStuckVehicle( + planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); + if (yield_stuck_detected && stuck_stop_line_idx_opt) { + auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { + if ( + default_stop_line_idx_opt && + fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { + stuck_stop_line_idx = default_stop_line_idx_opt.value(); + } + } else { + return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; + } + } + // if attention area is empty, collision/occlusion detection is impossible if (!first_attention_area_opt) { return IntersectionModule::Indecisive{"attention area is empty"}; @@ -1234,6 +1301,37 @@ bool IntersectionModule::checkStuckVehicle( &debug_data_); } +bool IntersectionModule::checkYieldStuckVehicle( + const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area) +{ + if (!first_attention_area) { + return false; + } + + const bool yield_stuck_detection_direction = [&]() { + return (turn_direction_ == "left" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.left) || + (turn_direction_ == "right" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.right) || + (turn_direction_ == "straight" && + planner_param_.stuck_vehicle.yield_stuck_turn_direction.straight); + }(); + if (!yield_stuck_detection_direction) { + return false; + } + + const auto & objects_ptr = planner_data->predicted_objects; + + const auto & ego_lane = path_lanelets.ego_or_entry2exit; + const auto ego_poly = ego_lane.polygon2d().basicPolygon(); + + return util::checkYieldStuckVehicleInIntersection( + objects_ptr, ego_poly, first_attention_area.value(), + planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, + planner_param_.stuck_vehicle.yield_stuck_distance_thr, &debug_data_); +} + autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 6ae734bd6e05b..a4804018d194b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -66,7 +66,8 @@ class IntersectionModule : public SceneModuleInterface bool left; bool right; bool straight; - } turn_direction; + }; + TurnDirection turn_direction; bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped @@ -77,6 +78,8 @@ class IntersectionModule : public SceneModuleInterface */ double timeout_private_area; bool enable_private_area_stuck_disregard; + double yield_stuck_distance_thr; + TurnDirection yield_stuck_turn_direction; } stuck_vehicle; struct CollisionDetection { @@ -147,6 +150,11 @@ class IntersectionModule : public SceneModuleInterface size_t stuck_stop_line_idx{0}; std::optional occlusion_stop_line_idx{std::nullopt}; }; + struct YieldStuckStop + { + size_t closest_idx{0}; + size_t stuck_stop_line_idx{0}; + }; struct NonOccludedCollisionStop { size_t closest_idx{0}; @@ -206,6 +214,7 @@ class IntersectionModule : public SceneModuleInterface using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line StuckStop, // detected stuck vehicle + YieldStuckStop, // detected yield stuck vehicle NonOccludedCollisionStop, // detected collision while FOV is clear FirstWaitBeforeOcclusion, // stop for a while before peeking to occlusion PeekingTowardOcclusion, // peeking into occlusion while collision is not detected @@ -288,6 +297,11 @@ class IntersectionModule : public SceneModuleInterface const std::shared_ptr & planner_data, const util::PathLanelets & path_lanelets); + bool checkYieldStuckVehicle( + const std::shared_ptr & planner_data, + const util::PathLanelets & path_lanelets, + const std::optional & first_attention_area); + autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( const lanelet::ConstLanelets & attention_area_lanelets, const lanelet::ConstLanelets & adjacent_lanelets, diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index ffc0b61880188..17df31ba3ffc9 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1108,6 +1108,45 @@ bool checkStuckVehicleInIntersection( return false; } +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) +{ + const auto first_attention_area_2d = lanelet::utils::to2D(first_attention_area); + Polygon2d first_attention_area_poly; + for (const auto & p : first_attention_area_2d) { + first_attention_area_poly.outer().emplace_back(p.x(), p.y()); + } + + for (const auto & object : objects_ptr->objects) { + if (!isTargetStuckVehicleType(object)) { + continue; // not target vehicle type + } + const auto obj_v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + if (obj_v_norm > stuck_vehicle_vel_thr) { + continue; // not stop vehicle + } + + const auto obj_footprint = tier4_autoware_utils::toPolygon2d(object); + + // check if the object is too close to the ego path + if (yield_stuck_distance_thr < bg::distance(ego_poly, obj_footprint)) { + continue; + } + + // check if the footprint is in the stuck detect area + const bool is_in_stuck_area = bg::within(obj_footprint, first_attention_area_poly); + if (is_in_stuck_area && debug_data) { + debug_data->yield_stuck_targets.objects.push_back(object); + return true; + } + } + return false; +} + Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist) { diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index a75545353fb7a..001aa63c7fe12 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -125,6 +125,12 @@ bool checkStuckVehicleInIntersection( const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, DebugData * debug_data); +bool checkYieldStuckVehicleInIntersection( + const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, + const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, + DebugData * debug_data); + Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 581f22904fd2b..11c8d7d8407b7 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -50,6 +50,7 @@ struct DebugData std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; From 4383d6bcfcd5df6ec4f0b15e28ea501c2024b7ba Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Sat, 14 Oct 2023 06:32:59 +0800 Subject: [PATCH 090/206] feat(duplicated_node_checker): add packages to check duplication of node names in ros2 (#5286) * add implementation for duplicated node checking Signed-off-by: Owen-Liuyuxuan * update the default parameters of system_error_monitor to include results from duplication check Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix typo in readme Signed-off-by: Owen-Liuyuxuan * update license Signed-off-by: Owen-Liuyuxuan * change module to the system module Signed-off-by: Owen-Liuyuxuan * follow json schema: 1. update code to start without default 2. add schema/config/readme/launch accordingly Signed-off-by: Owen-Liuyuxuan * add duplicated node checker to launch Signed-off-by: Owen-Liuyuxuan * style(pre-commit): autofix * fix var name to config for uniform launch Signed-off-by: Owen-Liuyuxuan * Update system/duplicated_node_checker/README.md * Update system/duplicated_node_checker/README.md --------- Signed-off-by: Owen-Liuyuxuan Co-authored-by: Owen-Liuyuxuan Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> --- .../launch/system.launch.xml | 7 ++ system/duplicated_node_checker/CMakeLists.txt | 20 +++++ system/duplicated_node_checker/README.md | 37 +++++++++ .../config/duplicated_node_checker.param.yaml | 3 + .../duplicated_node_checker_core.hpp | 54 +++++++++++++ .../launch/duplicated_node_checker.launch.xml | 7 ++ system/duplicated_node_checker/package.xml | 24 ++++++ .../duplicated_node_checker.schema.json | 31 +++++++ .../src/duplicated_node_checker_core.cpp | 81 +++++++++++++++++++ .../diagnostic_aggregator/system.param.yaml | 6 ++ .../config/system_error_monitor.param.yaml | 1 + ...ror_monitor.planning_simulation.param.yaml | 1 + 12 files changed, 272 insertions(+) create mode 100644 system/duplicated_node_checker/CMakeLists.txt create mode 100644 system/duplicated_node_checker/README.md create mode 100644 system/duplicated_node_checker/config/duplicated_node_checker.param.yaml create mode 100644 system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp create mode 100644 system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml create mode 100644 system/duplicated_node_checker/package.xml create mode 100644 system/duplicated_node_checker/schema/duplicated_node_checker.schema.json create mode 100644 system/duplicated_node_checker/src/duplicated_node_checker_core.cpp diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 016d613cfa72d..2de6a61547498 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -3,6 +3,7 @@ + @@ -84,6 +85,12 @@ + + + + + + diff --git a/system/duplicated_node_checker/CMakeLists.txt b/system/duplicated_node_checker/CMakeLists.txt new file mode 100644 index 0000000000000..6a8089fa85c96 --- /dev/null +++ b/system/duplicated_node_checker/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(duplicated_node_checker) + +find_package(autoware_cmake REQUIRED) +autoware_package() +find_package(rclcpp REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/duplicated_node_checker_core.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "duplicated_node_checker::DuplicatedNodeChecker" + EXECUTABLE duplicated_node_checker_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/duplicated_node_checker/README.md b/system/duplicated_node_checker/README.md new file mode 100644 index 0000000000000..81b53e36c9f6d --- /dev/null +++ b/system/duplicated_node_checker/README.md @@ -0,0 +1,37 @@ +# Duplicated Node Checker + +## Purpose + +This node monitors the ROS 2 environments and detect duplication of node names in the environment. +The result is published as diagnostics. + +### Standalone Startup + +```bash +ros2 launch duplicated_node_checker duplicated_node_checker.launch.xml +``` + +## Inner-workings / Algorithms + +The types of topic status and corresponding diagnostic status are following. + +| Duplication status | Diagnostic status | Description | +| --------------------- | ----------------- | -------------------------- | +| `OK` | OK | No duplication is detected | +| `Duplicated Detected` | ERROR | Duplication is detected | + +## Inputs / Outputs + +### Output + +| Name | Type | Description | +| -------------- | --------------------------------- | ------------------- | +| `/diagnostics` | `diagnostic_msgs/DiagnosticArray` | Diagnostics outputs | + +## Parameters + +{{ json_to_markdown("system/duplicated_node_checker/schema/duplicated_node_checker.schema.json") }} + +## Assumptions / Known limits + +TBD. diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml new file mode 100644 index 0000000000000..5b8c019de5a20 --- /dev/null +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -0,0 +1,3 @@ +/**: + ros__parameters: + update_rate: 10.0 diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp new file mode 100644 index 0000000000000..ec086058e9041 --- /dev/null +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -0,0 +1,54 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ +#define DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ +class DuplicatedNodeChecker : public rclcpp::Node +{ +public: + explicit DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options); + std::vector findIdenticalNames(const std::vector input_name_lists) + { + std::unordered_set unique_names; + std::vector identical_names; + for (auto name : input_name_lists) { + if (unique_names.find(name) != unique_names.end()) { + identical_names.push_back(name); + } else { + unique_names.insert(name); + } + } + return identical_names; + } + +private: + void onTimer(); + void produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + + diagnostic_updater::Updater updater_{this}; + rclcpp::TimerBase::SharedPtr timer_; +}; +} // namespace duplicated_node_checker + +#endif // DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ diff --git a/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml new file mode 100644 index 0000000000000..87f41f045545d --- /dev/null +++ b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/duplicated_node_checker/package.xml b/system/duplicated_node_checker/package.xml new file mode 100644 index 0000000000000..e2ba225b16330 --- /dev/null +++ b/system/duplicated_node_checker/package.xml @@ -0,0 +1,24 @@ + + + + duplicated_node_checker + 1.0.0 + A package to find out nodes with common names + Shumpei Wakabayashi + yliuhb + Apache 2.0 + + ament_cmake + autoware_cmake + + diagnostic_updater + rclcpp + rclcpp_components + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json new file mode 100644 index 0000000000000..6ee933ecf1a77 --- /dev/null +++ b/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json @@ -0,0 +1,31 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Duplicated Node Checker", + "type": "object", + "definitions": { + "duplicated_node_checker": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10, + "exclusiveMinimum": 2, + "description": "The scanning and update frequency of the checker." + } + }, + "required": ["update_rate"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/duplicated_node_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp new file mode 100644 index 0000000000000..c9aa483724532 --- /dev/null +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -0,0 +1,81 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "duplicated_node_checker/duplicated_node_checker_core.hpp" + +#include +#include + +#include +#include +#include + +namespace duplicated_node_checker +{ + +DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options) +: Node("duplicated_node_checker", node_options) +{ + double update_rate = declare_parameter("update_rate"); + updater_.setHardwareID("duplicated_node_checker"); + updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); + + const auto period_ns = rclcpp::Rate(update_rate).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&DuplicatedNodeChecker::onTimer, this)); +} + +std::string get_fullname_from_name_ns_pair(std::pair name_and_ns_pair) +{ + std::string full_name; + const std::string & name = name_and_ns_pair.first; + const std::string & ns = name_and_ns_pair.second; + if (ns.back() == '/') { + full_name = ns + name; + } else { + full_name = ns + "/" + name; + } + return full_name; +} + +void DuplicatedNodeChecker::onTimer() +{ + updater_.force_update(); +} + +void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + using diagnostic_msgs::msg::DiagnosticStatus; + + std::vector node_names = this->get_node_names(); + std::vector identical_names = findIdenticalNames(node_names); + std::string msg; + int level; + if (identical_names.size() > 0) { + msg = "Error"; + level = DiagnosticStatus::ERROR; + for (auto name : identical_names) { + stat.add("Duplicated Node Name", name); + } + } else { + msg = "OK"; + level = DiagnosticStatus::OK; + } + stat.summary(level, msg); +} + +} // namespace duplicated_node_checker + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(duplicated_node_checker::DuplicatedNodeChecker) diff --git a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml index 87cf767accc06..f6e13012957aa 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/system.param.yaml @@ -26,6 +26,12 @@ contains: ["service_log_checker"] timeout: 5.0 + duplicated_node_checker: + type: diagnostic_aggregator/GenericAnalyzer + path: duplicated_node_checker + contains: ["duplicated_node_checker"] + timeout: 5.0 + resource_monitoring: type: diagnostic_aggregator/AnalyzerGroup path: resource_monitoring 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 312a2e6186c42..1778f6594f0c3 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -39,6 +39,7 @@ /autoware/system/emergency_stop_operation: default /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/system/duplicated_node_checker: default /autoware/vehicle/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 88431dc406fe3..c396e2e9f5ed8 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 @@ -38,6 +38,7 @@ /autoware/system/node_alive_monitoring: default /autoware/system/emergency_stop_operation: default /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } + /autoware/system/duplicated_node_checker: default # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } /autoware/vehicle/node_alive_monitoring: default From d1a77d59fc6b676e94bd9ef62a644c310b676c3f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Sun, 15 Oct 2023 13:50:03 +0900 Subject: [PATCH 091/206] feat(intersection): ignore decelerating vehicle on amber traffic light (#5304) * merge attention area lanelets topologically Signed-off-by: Mamoru Sobue * query stop line for each merged detection lanelet Signed-off-by: Mamoru Sobue * first conflicting/attention area Signed-off-by: Mamoru Sobue * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue * ignore expectedToStopBeforeStopLine vehicle Signed-off-by: Mamoru Sobue * fix Signed-off-by: Mamoru Sobue --------- Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 8 +- .../src/debug.cpp | 5 + .../src/manager.cpp | 8 +- .../src/scene_intersection.cpp | 314 ++++++++-------- .../src/scene_intersection.hpp | 26 +- .../src/util.cpp | 334 +++++++++++------- .../src/util.hpp | 15 +- .../src/util_type.hpp | 59 +++- 8 files changed, 455 insertions(+), 314 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 30f4bc7b42a25..1a5143678ddce 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -48,9 +48,11 @@ use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity yield_on_green_traffic_light: - distance_to_assigned_lanelet_start: 10.0 - duration: 3.0 - range: 50.0 # [m] + distance_to_assigned_lanelet_start: 5.0 + duration: 2.0 + object_dist_to_stopline: 10.0 # [m] + ignore_on_amber_traffic_light: + object_expected_deceleration: 2.0 # [m/ss] occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 82f5b12966c12..3d5874c3c89e8 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -233,6 +233,11 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.conflicting_targets, "conflicting_targets", module_id_, now, 0.99, 0.4, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 93d67e786cff5..22fa57f20e79f 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -129,8 +129,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ns + ".collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start"); ip.collision_detection.yield_on_green_traffic_light.duration = getOrDeclareParameter( node, ns + ".collision_detection.yield_on_green_traffic_light.duration"); - ip.collision_detection.yield_on_green_traffic_light.range = getOrDeclareParameter( - node, ns + ".collision_detection.yield_on_green_traffic_light.range"); + ip.collision_detection.yield_on_green_traffic_light.object_dist_to_stopline = + getOrDeclareParameter( + node, ns + ".collision_detection.yield_on_green_traffic_light.object_dist_to_stopline"); + ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 76c2dc072dda9..b37b70f290ff6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1103,7 +1103,6 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); - const auto & attention_lanelets = intersection_lanelets.attention(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); const auto & occlusion_attention_area = intersection_lanelets.occlusion_attention_area(); @@ -1120,8 +1119,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - const auto target_objects = - filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); + auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); // If there are any vehicles on the attention area when ego entered the intersection on green // light, do pseudo collision detection because the vehicles are very slow and no collisions may @@ -1145,11 +1143,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } if (initial_green_light_observed_time_) { const auto now = clock_->now(); - const bool exist_close_vehicles = std::any_of( - target_objects.objects.begin(), target_objects.objects.end(), [&](const auto & object) { - return tier4_autoware_utils::calcDistance3d( - object.kinematics.initial_pose_with_covariance.pose, current_pose) < - planner_param_.collision_detection.yield_on_green_traffic_light.range; + const bool exist_close_vehicles = + std::any_of(target_objects.all.begin(), target_objects.all.end(), [&](const auto & object) { + return object.dist_to_stop_line.has_value() && + object.dist_to_stop_line.value() < + planner_param_.collision_detection.yield_on_green_traffic_light + .object_dist_to_stopline; }); if ( exist_close_vehicles && @@ -1168,7 +1167,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getDuration()); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, + *path, &target_objects, path_lanelets, closest_idx, std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( @@ -1195,16 +1194,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - std::vector blocking_attention_objects; - std::copy_if( - target_objects.objects.begin(), target_objects.objects.end(), - std::back_inserter(blocking_attention_objects), - [thresh = planner_param_.occlusion.ignore_parked_vehicle_speed_threshold](const auto & object) { - return std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; - }); - debug_data_.blocking_attention_objects.objects = blocking_attention_objects; + const auto blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + } + // debug_data_.blocking_attention_objects.objects = blocking_attention_objects; const bool is_occlusion_cleared = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? isOcclusionCleared( @@ -1332,18 +1326,17 @@ bool IntersectionModule::checkYieldStuckVehicle( planner_param_.stuck_vehicle.yield_stuck_distance_thr, &debug_data_); } -autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, +util::TargetObjects IntersectionModule::generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const { - using lanelet::utils::getArcCoordinates; - using lanelet::utils::getPolygonFromArcLength; - const auto & objects_ptr = planner_data_->predicted_objects; // extract target objects - autoware_auto_perception_msgs::msg::PredictedObjects target_objects; + util::TargetObjects target_objects; target_objects.header = objects_ptr->header; + const auto & attention_lanelets = intersection_lanelets.attention(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stop_lines(); + const auto & adjacent_lanelets = intersection_lanelets.adjacent(); for (const auto & object : objects_ptr->objects) { // ignore non-vehicle type objects, such as pedestrian. if (!isTargetCollisionVehicleType(object)) { @@ -1352,49 +1345,72 @@ autoware_auto_perception_msgs::msg::PredictedObjects IntersectionModule::filterT // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); - const auto is_in_adjacent_lanelets = util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - adjacent_lanelets, planner_param_.common.attention_area_angle_thr, + const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold); - if (is_in_adjacent_lanelets) { + planner_param_.common.attention_area_margin, false); + if (belong_adjacent_lanelet_id) { continue; } + const auto is_parked_vehicle = + std::fabs(object.kinematics.initial_twist_with_covariance.twist.linear.x) < + planner_param_.occlusion.ignore_parked_vehicle_speed_threshold; + auto & container = is_parked_vehicle ? target_objects.parked_attention_objects + : target_objects.attention_objects; if (intersection_area) { const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); - const auto is_in_intersection_area = bg::within(obj_poly, intersection_area_2d); - if (is_in_intersection_area) { - target_objects.objects.push_back(object); - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { - target_objects.objects.push_back(object); + const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, planner_param_.common.attention_area_angle_thr, + planner_param_.common.consider_wrong_direction_vehicle, + planner_param_.common.attention_area_margin, is_parked_vehicle); + if (belong_attention_lanelet_id) { + const auto id = belong_adjacent_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); + } else if (bg::within(obj_poly, intersection_area_2d)) { + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = std::nullopt; + target_object.stop_line = std::nullopt; + container.push_back(target_object); } - } else if (util::checkAngleForTargetLanelets( - object_direction, object.kinematics.initial_twist_with_covariance.twist.linear.x, - attention_area_lanelets, planner_param_.common.attention_area_angle_thr, + } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( + object_direction, attention_lanelets, + planner_param_.common.attention_area_angle_thr, planner_param_.common.consider_wrong_direction_vehicle, - planner_param_.common.attention_area_margin, - planner_param_.occlusion.ignore_parked_vehicle_speed_threshold)) { + planner_param_.common.attention_area_margin, is_parked_vehicle); + belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before - target_objects.objects.push_back(object); + const auto id = belong_attention_lanelet_id.value(); + util::TargetObject target_object; + target_object.object = object; + target_object.attention_lanelet = attention_lanelets.at(id); + target_object.stop_line = attention_lanelet_stoplines.at(id); + container.push_back(target_object); } } + for (const auto & object : target_objects.attention_objects) { + target_objects.all.push_back(object); + } + for (const auto & object : target_objects.parked_attention_objects) { + target_objects.all.push_back(object); + } + for (auto & object : target_objects.all) { + object.calc_dist_to_stop_line(); + } return target_objects; } bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, - const util::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, - const util::TrafficPrioritizedLevel & traffic_prioritized_level) + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1408,17 +1424,16 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.use_upstream_velocity, planner_param_.collision_detection.minimum_upstream_velocity); const double passing_time = time_distance_array.back().first; - auto target_objects = objects; - util::cutPredictPathWithDuration(&target_objects, clock_, passing_time); + util::cutPredictPathWithDuration(target_objects, clock_, passing_time); const auto & concat_lanelets = path_lanelets.all; const auto closest_arc_coords = getArcCoordinates( concat_lanelets, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); const auto & ego_lane = path_lanelets.ego_or_entry2exit; debug_data_.ego_lane = ego_lane.polygon3d(); - const auto ego_poly = ego_lane.polygon2d().basicPolygon(); - // check collision between predicted_path and ego_area + + // change TTC margin based on ego traffic light color const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { return std::make_pair( @@ -1434,8 +1449,33 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_start_margin_time, planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); + const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { + if (!target_object.dist_to_stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + if (dist_to_stop_line < 0) { + return false; + } + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + return dist_to_stop_line > braking_distance; + }; + + // check collision between predicted_path and ego_area bool collision_detected = false; - for (const auto & object : target_objects.objects) { + for (const auto & target_object : target_objects->all) { + const auto & object = target_object.object; + // If the vehicle is expected to stop before their stopline, ignore + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && + expectedToStopBeforeStopLine(target_object)) { + debug_data_.amber_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < @@ -1532,9 +1572,8 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - blocking_attention_objects, + const std::vector & lane_divisions, + const std::vector & blocking_attention_objects, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1660,27 +1699,24 @@ bool IntersectionModule::isOcclusionCleared( // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded std::vector> blocking_polygons; for (const auto & blocking_attention_object : blocking_attention_objects) { - const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object); + const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); findCommonCvPolygons(obj_poly.outer(), blocking_polygons); } for (const auto & blocking_polygon : blocking_polygons) { cv::fillPoly(attention_mask, blocking_polygon, cv::Scalar(BLOCKED), cv::LINE_AA); } - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - for (const auto & division : divisions) { - bool blocking_vehicle_found = false; - for (const auto & point_it : division) { - const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); - if (!valid) continue; - if (blocking_vehicle_found) { - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - continue; - } - if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { - blocking_vehicle_found = true; - occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - } + for (const auto & division : lane_divisions) { + bool blocking_vehicle_found = false; + for (const auto & point_it : division) { + const auto [valid, idx_x, idx_y] = coord2index(point_it.x(), point_it.y()); + if (!valid) continue; + if (blocking_vehicle_found) { + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; + continue; + } + if (attention_mask.at(height - 1 - idx_y, idx_x) == BLOCKED) { + blocking_vehicle_found = true; + occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; } } } @@ -1753,84 +1789,78 @@ bool IntersectionModule::isOcclusionCleared( } } - auto findNearestPointToProjection = - [](lanelet::ConstLineString2d division, const Point2d & projection, const double dist_thresh) { - double min_dist = std::numeric_limits::infinity(); - auto nearest = division.end(); - for (auto it = division.begin(); it != division.end(); it++) { - const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); - if (dist < min_dist) { - min_dist = dist; - nearest = it; - } - if (dist < dist_thresh) { - break; - } + auto findNearestPointToProjection = []( + const lanelet::ConstLineString3d & division, + const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; } - return nearest; - }; + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; struct NearestOcclusionPoint { - int lane_id; int64 division_index; double dist; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; } nearest_occlusion_point; double min_dist = std::numeric_limits::infinity(); - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - const auto lane_id = lane_division.lane_id; - for (unsigned division_index = 0; division_index < divisions.size(); ++division_index) { - const auto & division = divisions.at(division_index); - LineString2d division_linestring; - auto division_point_it = division.begin(); - division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); - for (auto point_it = division.begin(); point_it != division.end(); point_it++) { - if ( - std::hypot( - point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < - 3.0 /* rough tick for computational cost */) { - continue; - } - division_linestring.emplace_back(point_it->x(), point_it->y()); - division_point_it = point_it; - } - - // find the intersection point of lane_line and path - std::vector intersection_points; - boost::geometry::intersection(division_linestring, path_linestring, intersection_points); - if (intersection_points.empty()) { + for (unsigned division_index = 0; division_index < lane_divisions.size(); ++division_index) { + const auto & division = lane_divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot(point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { continue; } - const auto & projection_point = intersection_points.at(0); - const auto projection_it = - findNearestPointToProjection(division, projection_point, resolution); - if (projection_it == division.end()) { - continue; + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + // TODO(Mamoru Sobue): add handling for blocking vehicles + if (!valid) continue; + const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); + if (pixel == BLOCKED) { + break; } - double acc_dist = 0.0; - auto acc_dist_it = projection_it; - for (auto point_it = projection_it; point_it != division.end(); point_it++) { - const double dist = - std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); - acc_dist += dist; - acc_dist_it = point_it; - const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - // TODO(Mamoru Sobue): add handling for blocking vehicles - if (!valid) continue; - const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); - if (pixel == BLOCKED) { - break; - } - if (pixel == OCCLUDED) { - if (acc_dist < min_dist) { - min_dist = acc_dist; - nearest_occlusion_point = { - lane_id, std::distance(division.begin(), point_it), acc_dist, - tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), - tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; - } + if (pixel == OCCLUDED) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; } } } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index a4804018d194b..beb13ef7bef7a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -109,8 +109,12 @@ class IntersectionModule : public SceneModuleInterface { double distance_to_assigned_lanelet_start; double duration; - double range; + double object_dist_to_stopline; } yield_on_green_traffic_light; + struct IgnoreOnAmberTrafficLight + { + double object_expected_deceleration; + } ignore_on_amber_traffic_light; } collision_detection; struct Occlusion { @@ -266,7 +270,8 @@ class IntersectionModule : public SceneModuleInterface // for occlusion detection const bool enable_occlusion_detection_; - std::optional> occlusion_attention_divisions_{std::nullopt}; + std::optional> occlusion_attention_divisions_{ + std::nullopt}; StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; @@ -302,17 +307,15 @@ class IntersectionModule : public SceneModuleInterface const util::PathLanelets & path_lanelets, const std::optional & first_attention_area); - autoware_auto_perception_msgs::msg::PredictedObjects filterTargetObjects( - const lanelet::ConstLanelets & attention_area_lanelets, - const lanelet::ConstLanelets & adjacent_lanelets, + util::TargetObjects generateTargetObjects( + const util::IntersectionLanelets & intersection_lanelets, const std::optional & intersection_area) const; bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, - const util::PathLanelets & path_lanelets, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, - const util::TrafficPrioritizedLevel & traffic_prioritized_level); + util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, + const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, @@ -320,9 +323,8 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, - const std::vector & lane_divisions, - const std::vector & - parked_attention_objects, + const std::vector & lane_divisions, + const std::vector & blocking_attention_objects, const double occlusion_dist_thr); /* diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 17df31ba3ffc9..08c818c2e49ce 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -239,7 +239,8 @@ static std::optional getFirstPointInsidePolygonByFootprint( return std::nullopt; } -static std::optional> +static std::optional> getFirstPointInsidePolygonsByFootprint( const std::vector & polygons, const InterpolatedPathInfo & interpolated_path_info, @@ -252,12 +253,11 @@ getFirstPointInsidePolygonsByFootprint( const auto & pose = path_ip.points.at(i).point.pose; const auto path_footprint = tier4_autoware_utils::transformVector(footprint, tier4_autoware_utils::pose2transform(pose)); - for (const auto & polygon : polygons) { - const auto area_2d = lanelet::utils::to2D(polygon).basicPolygon(); + for (size_t j = 0; j < polygons.size(); ++j) { + const auto area_2d = lanelet::utils::to2D(polygons.at(j)).basicPolygon(); const bool is_in_polygon = bg::intersects(area_2d, path_footprint); if (is_in_polygon) { - return std::make_optional>( - i, polygon); + return std::make_optional>(i, j); } } } @@ -570,6 +570,101 @@ static std::vector getPolygon3dFromLanelets( return polys; } +static std::string getTurnDirection(lanelet::ConstLanelet lane) +{ + return lane.attributeOr("turn_direction", "else"); +} + +/** + * @param pair lanelets and the vector of original lanelets in topological order (not reversed as + *in generateDetectionLaneDivisions()) + **/ +static std::pair> +mergeLaneletsByTopologicalSort( + const lanelet::ConstLanelets & lanelets, + const lanelet::routing::RoutingGraphPtr routing_graph_ptr) +{ + const int n_node = lanelets.size(); + std::vector> adjacency(n_node); + for (int dst = 0; dst < n_node; ++dst) { + adjacency[dst].resize(n_node); + for (int src = 0; src < n_node; ++src) { + adjacency[dst][src] = false; + } + } + std::set lanelet_ids; + std::unordered_map id2ind; + std::unordered_map ind2id; + std::unordered_map id2lanelet; + int ind = 0; + for (const auto & lanelet : lanelets) { + lanelet_ids.insert(lanelet.id()); + const auto id = lanelet.id(); + id2ind[id] = ind; + ind2id[ind] = id; + id2lanelet[id] = lanelet; + ind++; + } + for (const auto & lanelet : lanelets) { + const auto & followings = routing_graph_ptr->following(lanelet); + const int dst = lanelet.id(); + for (const auto & following : followings) { + if (const int src = following.id(); lanelet_ids.find(src) != lanelet_ids.end()) { + adjacency[(id2ind[src])][(id2ind[dst])] = true; + } + } + } + // terminal node + std::map> branches; + auto has_no_previous = [&](const int node) { + for (int dst = 0; dst < n_node; dst++) { + if (adjacency[dst][node]) { + return false; + } + } + return true; + }; + for (int src = 0; src < n_node; src++) { + if (!has_no_previous(src)) { + continue; + } + branches[(ind2id[src])] = std::vector{}; + auto & branch = branches[(ind2id[src])]; + lanelet::Id node_iter = ind2id[src]; + while (true) { + const auto & destinations = adjacency[(id2ind[node_iter])]; + // NOTE: assuming detection lanelets have only one previous lanelet + const auto next = std::find(destinations.begin(), destinations.end(), true); + if (next == destinations.end()) { + branch.push_back(node_iter); + break; + } + branch.push_back(node_iter); + node_iter = ind2id[std::distance(destinations.begin(), next)]; + } + } + for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { + auto & branch = it->second; + std::reverse(branch.begin(), branch.end()); + } + lanelet::ConstLanelets merged; + std::vector originals; + for (const auto & [id, sub_ids] : branches) { + if (sub_ids.size() == 0) { + continue; + } + lanelet::ConstLanelets merge; + originals.push_back(lanelet::ConstLanelets({})); + auto & original = originals.back(); + for (const auto sub_id : sub_ids) { + merge.push_back(id2lanelet[sub_id]); + original.push_back(id2lanelet[sub_id]); + } + merged.push_back(lanelet::utils::combineLaneletsShape(merge)); + } + return {merged, originals}; +} + IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, @@ -707,14 +802,52 @@ IntersectionLanelets getObjectiveLanelets( } } } + lanelet::ConstLanelets occlusion_detection_and_preceding_lanelets_wo_turn_direction; + for (const auto & ll : occlusion_detection_and_preceding_lanelets) { + const auto turn_direction = getTurnDirection(ll); + if (turn_direction == "left" || turn_direction == "right") { + continue; + } + occlusion_detection_and_preceding_lanelets_wo_turn_direction.push_back(ll); + } + + auto [attention_lanelets, original_attention_lanelet_sequences] = + mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); IntersectionLanelets result; - result.attention_ = std::move(detection_and_preceding_lanelets); + result.attention_ = std::move(attention_lanelets); + for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { + // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that + // back() exists. + std::optional stop_line{std::nullopt}; + for (auto it = original_attention_lanelet_seq.rbegin(); + it != original_attention_lanelet_seq.rend(); ++it) { + const auto traffic_lights = it->regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + break; + } + if (stop_line) break; + } + result.attention_stop_lines_.push_back(stop_line); + } result.attention_non_preceding_ = std::move(detection_lanelets); + // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed + for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { + result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); - result.occlusion_attention_ = std::move(occlusion_detection_and_preceding_lanelets); - // compoundPolygon3d + // NOTE: occlusion_attention is not inverted here + // TODO(Mamoru Sobue): apply mergeLaneletsByTopologicalSort for occlusion lanelets as well and + // then trim part of them based on curvature threshold + result.occlusion_attention_ = + std::move(occlusion_detection_and_preceding_lanelets_wo_turn_direction); + + // NOTE: to properly update(), each element in conflicting_/conflicting_area_, + // attention_non_preceding_/attention_non_preceding_area_ need to be matched result.attention_area_ = getPolygon3dFromLanelets(result.attention_); result.attention_non_preceding_area_ = getPolygon3dFromLanelets(result.attention_non_preceding_); result.conflicting_area_ = getPolygon3dFromLanelets(result.conflicting_); @@ -723,11 +856,6 @@ IntersectionLanelets getObjectiveLanelets( return result; } -static std::string getTurnDirection(lanelet::ConstLanelet lane) -{ - return lane.attributeOr("turn_direction", "else"); -} - bool isOverTargetIndex( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, const geometry_msgs::msg::Pose & current_pose, const int target_idx) @@ -893,21 +1021,17 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) return *std::max_element(curvatures_positive.begin(), curvatures_positive.end()); } -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets_all, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds) { using lanelet::utils::getCenterlineWithOffset; - using lanelet::utils::to2D; // (0) remove left/right lanelet lanelet::ConstLanelets detection_lanelets; for (const auto & detection_lanelet : detection_lanelets_all) { - const auto turn_direction = getTurnDirection(detection_lanelet); - if (turn_direction.compare("left") == 0 || turn_direction.compare("right") == 0) { - continue; - } + // TODO(Mamoru Sobue): instead of ignoring, only trim straight part of lanelet const auto fine_centerline = lanelet::utils::generateFineCenterline(detection_lanelet, curvature_calculation_ds); const double highest_curvature = getHighestCurvature(fine_centerline); @@ -918,111 +1042,34 @@ std::vector generateDetectionLaneDivisions( } // (1) tsort detection_lanelets - // generate adjacency matrix - // if lanelet1 => lanelet2; then adjacency[lanelet2][lanelet1] = true - const int n_node = detection_lanelets.size(); - std::vector> adjacency(n_node); - for (int dst = 0; dst < n_node; ++dst) { - adjacency[dst].resize(n_node); - for (int src = 0; src < n_node; ++src) { - adjacency[dst][src] = false; - } - } - std::set detection_lanelet_ids; - std::unordered_map id2ind, ind2id; - std::unordered_map id2lanelet; - int ind = 0; - for (const auto & detection_lanelet : detection_lanelets) { - detection_lanelet_ids.insert(detection_lanelet.id()); - const int id = detection_lanelet.id(); - id2ind[id] = ind; - ind2id[ind] = id; - id2lanelet[id] = detection_lanelet; - ind++; - } - for (const auto & detection_lanelet : detection_lanelets) { - const auto & followings = routing_graph_ptr->following(detection_lanelet); - const int dst = detection_lanelet.id(); - for (const auto & following : followings) { - if (const int src = following.id(); - detection_lanelet_ids.find(src) != detection_lanelet_ids.end()) { - adjacency[(id2ind[src])][(id2ind[dst])] = true; - } - } - } - // terminal node - std::map> branches; - auto has_no_previous = [&](const int node) { - for (int dst = 0; dst < n_node; dst++) { - if (adjacency[dst][node]) { - return false; - } - } - return true; - }; - for (int src = 0; src < n_node; src++) { - if (!has_no_previous(src)) { - continue; - } - branches[(ind2id[src])] = std::vector{}; - auto & branch = branches[(ind2id[src])]; - int node_iter = ind2id[src]; - while (true) { - const auto & destinations = adjacency[(id2ind[node_iter])]; - // NOTE: assuming detection lanelets have only one previous lanelet - const auto next = std::find(destinations.begin(), destinations.end(), true); - if (next == destinations.end()) { - branch.push_back(node_iter); - break; - } - branch.push_back(node_iter); - node_iter = ind2id[std::distance(destinations.begin(), next)]; - } - } - for (decltype(branches)::iterator it = branches.begin(); it != branches.end(); it++) { - auto & branch = it->second; - std::reverse(branch.begin(), branch.end()); - } + const auto [merged_detection_lanelets, originals] = + mergeLaneletsByTopologicalSort(detection_lanelets, routing_graph_ptr); // (2) merge each branch to one lanelet // NOTE: somehow bg::area() for merged lanelet does not work, so calculate it here - std::unordered_map> merged_branches; - for (const auto & [src, branch] : branches) { - lanelet::Points3d lefts; - lanelet::Points3d rights; + std::vector> merged_lanelet_with_area; + for (unsigned i = 0; i < merged_detection_lanelets.size(); ++i) { + const auto & merged_detection_lanelet = merged_detection_lanelets.at(i); + const auto & original = originals.at(i); double area = 0; - for (const auto & lane_id : branch) { - const auto lane = id2lanelet[lane_id]; - for (const auto & left_point : lane.leftBound()) { - lefts.push_back(lanelet::Point3d(left_point)); - } - for (const auto & right_point : lane.rightBound()) { - rights.push_back(lanelet::Point3d(right_point)); - } - area += bg::area(lane.polygon2d().basicPolygon()); + for (const auto & partition : original) { + area += bg::area(partition.polygon2d().basicPolygon()); } - lanelet::LineString3d right = lanelet::LineString3d(lanelet::InvalId, lefts).invert(); - lanelet::LineString3d left = lanelet::LineString3d(lanelet::InvalId, rights).invert(); - lanelet::Lanelet merged = lanelet::Lanelet(lanelet::InvalId, left, right); - merged_branches[src] = std::make_pair(merged, area); + merged_lanelet_with_area.emplace_back(merged_detection_lanelet, area); } // (3) discretize each merged lanelet - std::vector detection_divisions; - for (const auto & [last_lane_id, branch] : merged_branches) { - DiscretizedLane detection_division; - detection_division.lane_id = last_lane_id; - const auto detection_lanelet = branch.first; - const double area = branch.second; - const double length = bg::length(detection_lanelet.centerline()); + std::vector detection_divisions; + for (const auto & [merged_lanelet, area] : merged_lanelet_with_area) { + const double length = bg::length(merged_lanelet.centerline()); const double width = area / length; - auto & divisions = detection_division.divisions; for (int i = 0; i < static_cast(width / resolution); ++i) { const double offset = resolution * i - width / 2; - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, offset, resolution))); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, offset, resolution).invert()); } - divisions.push_back(to2D(getCenterlineWithOffset(detection_lanelet, width / 2, resolution))); - detection_divisions.push_back(detection_division); + detection_divisions.push_back( + getCenterlineWithOffset(merged_lanelet, width / 2, resolution).invert()); } return detection_divisions; } @@ -1186,13 +1233,13 @@ Polygon2d generateStuckVehicleDetectAreaPolygon( return polygon; } -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold) +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle) { - for (const auto & ll : target_lanelets) { + for (unsigned i = 0; i < target_lanelets.size(); ++i) { + const auto & ll = target_lanelets.at(i); if (!lanelet::utils::isInLanelet(pose, ll, dist_margin)) { continue; } @@ -1201,39 +1248,38 @@ bool checkAngleForTargetLanelets( const double angle_diff = tier4_autoware_utils::normalizeRadian(ll_angle - pose_angle, -M_PI); if (consider_wrong_direction_vehicle) { if (std::fabs(angle_diff) > 1.57 || std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } } else { if (std::fabs(angle_diff) < detection_area_angle_thr) { - return true; + return std::make_optional(i); } // NOTE: sometimes parked vehicle direction is reversed even if its longitudinal velocity is // positive if ( - std::fabs(longitudinal_velocity) < parked_vehicle_speed_threshold && - (std::fabs(angle_diff) < detection_area_angle_thr || - (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { - return true; + is_parked_vehicle && (std::fabs(angle_diff) < detection_area_angle_thr || + (std::fabs(angle_diff + M_PI) < detection_area_angle_thr))) { + return std::make_optional(i); } } } - return false; + return std::nullopt; } void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr) + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) { const rclcpp::Time current_time = clock->now(); - for (auto & object : objects_ptr->objects) { // each objects - for (auto & predicted_path : object.kinematics.predicted_paths) { // each predicted paths + for (auto & target_object : target_objects->all) { // each objects + for (auto & predicted_path : + target_object.object.kinematics.predicted_paths) { // each predicted paths const auto origin_path = predicted_path; predicted_path.path.clear(); for (size_t k = 0; k < origin_path.path.size(); ++k) { // each path points const auto & predicted_pose = origin_path.path.at(k); const auto predicted_time = - rclcpp::Time(objects_ptr->header.stamp) + + rclcpp::Time(target_objects->header.stamp) + rclcpp::Duration(origin_path.time_step) * static_cast(k); if ((predicted_time - current_time).seconds() < time_thr) { predicted_path.path.push_back(predicted_pose); @@ -1342,14 +1388,16 @@ void IntersectionLanelets::update( auto first = getFirstPointInsidePolygonsByFootprint(conflicting_area_, interpolated_path_info, footprint); if (first) { - first_conflicting_area_ = first.value().second; + first_conflicting_lane_ = conflicting_.at(first.value().second); + first_conflicting_area_ = conflicting_area_.at(first.value().second); } } if (!first_attention_area_) { - auto first = - getFirstPointInsidePolygonsByFootprint(attention_area_, interpolated_path_info, footprint); + auto first = getFirstPointInsidePolygonsByFootprint( + attention_non_preceding_area_, interpolated_path_info, footprint); if (first) { - first_attention_area_ = first.value().second; + first_attention_lane_ = attention_non_preceding_.at(first.value().second); + first_attention_area_ = attention_non_preceding_area_.at(first.value().second); } } } @@ -1435,5 +1483,23 @@ std::optional generatePathLanelets( return path_lanelets; } +void TargetObject::calc_dist_to_stop_line() +{ + if (!attention_lanelet || !stop_line) { + return; + } + const auto attention_lanelet_val = attention_lanelet.value(); + const auto object_arc_coords = lanelet::utils::getArcCoordinates( + {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); + const auto stop_line_val = stop_line.value(); + geometry_msgs::msg::Pose stopline_center; + stopline_center.position.x = (stop_line_val.front().x() + stop_line_val.back().x()) / 2.0; + stopline_center.position.y = (stop_line_val.front().y() + stop_line_val.back().y()) / 2.0; + stopline_center.position.z = (stop_line_val.front().z() + stop_line_val.back().z()) / 2.0; + const auto stopline_arc_coords = + lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); + dist_to_stop_line = (stopline_arc_coords.length - object_arc_coords.length); +} + } // namespace util } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 001aa63c7fe12..ef658a25fce55 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -107,7 +107,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); TrafficPrioritizedLevel getTrafficPrioritizedLevel( lanelet::ConstLanelet lane, const std::map & tl_infos); -std::vector generateDetectionLaneDivisions( +std::vector generateDetectionLaneDivisions( lanelet::ConstLanelets detection_lanelets, const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution, const double curvature_threshold, const double curvature_calculation_ds); @@ -134,15 +134,14 @@ bool checkYieldStuckVehicleInIntersection( Polygon2d generateStuckVehicleDetectAreaPolygon( const util::PathLanelets & path_lanelets, const double stuck_vehicle_detect_dist); -bool checkAngleForTargetLanelets( - const geometry_msgs::msg::Pose & pose, const double longitudinal_velocity, - const lanelet::ConstLanelets & target_lanelets, const double detection_area_angle_thr, - const bool consider_wrong_direction_vehicle, const double dist_margin, - const double parked_vehicle_speed_threshold); +std::optional checkAngleForTargetLanelets( + const geometry_msgs::msg::Pose & pose, const lanelet::ConstLanelets & target_lanelets, + const double detection_area_angle_thr, const bool consider_wrong_direction_vehicle, + const double dist_margin, const bool is_parked_vehicle); void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, - const rclcpp::Clock::SharedPtr clock, const double time_thr); + util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, + const double time_thr); TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 11c8d7d8407b7..3f09b54f88be4 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -49,6 +49,7 @@ struct DebugData std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; @@ -77,6 +78,10 @@ struct IntersectionLanelets { return is_prioritized_ ? attention_non_preceding_ : attention_; } + const std::vector> & attention_stop_lines() const + { + return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; + } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const @@ -96,38 +101,48 @@ struct IntersectionLanelets { return occlusion_attention_area_; } + const std::optional & first_conflicting_lane() const + { + return first_conflicting_lane_; + } const std::optional & first_conflicting_area() const { return first_conflicting_area_; } + const std::optional & first_attention_lane() const + { + return first_attention_lane_; + } const std::optional & first_attention_area() const { return first_attention_area_; } - lanelet::ConstLanelets attention_; + lanelet::ConstLanelets attention_; // topologically merged lanelets + std::vector> + attention_stop_lines_; // the stop lines for each attention_ lanelets lanelet::ConstLanelets attention_non_preceding_; + std::vector> + attention_non_preceding_stop_lines_; // the stop lines for each attention_non_preceding_ + // lanelets lanelet::ConstLanelets conflicting_; lanelet::ConstLanelets adjacent_; - lanelet::ConstLanelets occlusion_attention_; // for occlusion detection - std::vector attention_area_; + lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets + std::vector occlusion_attention_size_; // the area() of each occlusion attention lanelets + std::vector attention_area_; // topologically merged lanelets std::vector attention_non_preceding_area_; std::vector conflicting_area_; std::vector adjacent_area_; - std::vector occlusion_attention_area_; + std::vector + occlusion_attention_area_; // topologically merged lanelets // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. + std::optional first_conflicting_lane_{std::nullopt}; + std::optional first_conflicting_area_{std::nullopt}; + std::optional first_attention_lane_{std::nullopt}; + std::optional first_attention_area_{std::nullopt}; bool is_prioritized_ = false; - std::optional first_conflicting_area_ = std::nullopt; - std::optional first_attention_area_ = std::nullopt; -}; - -struct DiscretizedLane -{ - int lane_id{0}; - // discrete fine lines from left to right - std::vector divisions{}; }; struct IntersectionStopLines @@ -161,6 +176,24 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the path }; +struct TargetObject +{ + autoware_auto_perception_msgs::msg::PredictedObject object; + std::optional attention_lanelet{std::nullopt}; + std::optional stop_line{std::nullopt}; + std::optional dist_to_stop_line{std::nullopt}; + void calc_dist_to_stop_line(); +}; + +struct TargetObjects +{ + std_msgs::msg::Header header; + std::vector attention_objects; + std::vector parked_attention_objects; + std::vector intersection_area_objects; + std::vector all; // TODO(Mamoru Sobue): avoid copy +}; + enum class TrafficPrioritizedLevel { // The target lane's traffic signal is red or the ego's traffic signal has an arrow. FULLY_PRIORITIZED = 0, From 1c5ca20218a1968888e42fd30cf535aa24dcc79f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 15 Oct 2023 19:32:06 +0900 Subject: [PATCH 092/206] feat(lane_change): add rss paramas for stuck (#5300) Signed-off-by: kosuke55 --- .../config/lane_change/lane_change.param.yaml | 8 +++++ .../scene_module/lane_change/base_class.hpp | 3 +- .../scene_module/lane_change/normal.hpp | 3 +- .../lane_change/lane_change_module_data.hpp | 1 + .../src/scene_module/lane_change/manager.cpp | 15 +++++++++ .../src/scene_module/lane_change/normal.cpp | 33 +++++++++++++------ 6 files changed, 51 insertions(+), 12 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index e7f3b51bd2d26..d3c0a22e867f9 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -45,6 +45,14 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.6 + stuck: + expected_front_deceleration: -1.0 + expected_rear_deceleration: -1.0 + rear_vehicle_reaction_time: 2.0 + rear_vehicle_safety_time_margin: 1.0 + lateral_distance_max_threshold: 2.0 + longitudinal_distance_min_threshold: 3.0 + longitudinal_velocity_delta_time: 0.8 # lane expansion for object filtering lane_expansion: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index a3469dee2909b..26f68e69b3728 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -230,7 +230,8 @@ class LaneChangeBase virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, Direction direction, - LaneChangePaths * candidate_paths, const bool check_safety) const = 0; + 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; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 4f1eb7340fdff..e9e361e1c39b8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -138,6 +138,7 @@ class NormalLaneChange : public LaneChangeBase bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; TurnSignalInfo calcTurnSignalInfo() override; @@ -146,7 +147,7 @@ class NormalLaneChange : public LaneChangeBase PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, CollisionCheckDebugMap & debug_data) const; LaneChangeTargetObjectIndices filterObject( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 24a507d1f8695..8b9ef52394cdd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -85,6 +85,7 @@ struct LaneChangeParameters bool allow_loose_check_for_cancel{true}; utils::path_safety_checker::RSSparams rss_params{}; utils::path_safety_checker::RSSparams rss_params_for_abort{}; + utils::path_safety_checker::RSSparams rss_params_for_stuck{}; // abort LaneChangeCancelParameters cancel{}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index e33a6c4b05ee3..3d7cf07308a79 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -128,6 +128,21 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.rss_params_for_abort.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.cancel.lateral_distance_max_threshold")); + p.rss_params_for_stuck.longitudinal_distance_min_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_distance_min_threshold")); + p.rss_params_for_stuck.longitudinal_velocity_delta_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.longitudinal_velocity_delta_time")); + p.rss_params_for_stuck.front_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_front_deceleration")); + p.rss_params_for_stuck.rear_vehicle_deceleration = getOrDeclareParameter( + *node, parameter("safety_check.stuck.expected_rear_deceleration")); + p.rss_params_for_stuck.rear_vehicle_reaction_time = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_reaction_time")); + p.rss_params_for_stuck.rear_vehicle_safety_time_margin = getOrDeclareParameter( + *node, parameter("safety_check.stuck.rear_vehicle_safety_time_margin")); + p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( + *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + // target object { std::string ns = "lane_change.target_object."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c332438dd31a5..d155b204f62d6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -83,8 +83,17 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) // find candidate paths LaneChangePaths valid_paths{}; - const auto found_safe_path = - getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); + const bool is_stuck = isVehicleStuckByObstacle(current_lanes); + bool found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, lane_change_parameters_->rss_params, + is_stuck); + // if no safe path is found and ego is stuck, try to find a path with a small margin + if (!found_safe_path && is_stuck) { + found_safe_path = getLaneChangePaths( + current_lanes, target_lanes, direction_, &valid_paths, + lane_change_parameters_->rss_params_for_stuck, is_stuck); + } + debug_valid_path_ = valid_paths; if (valid_paths.empty()) { @@ -1006,7 +1015,9 @@ bool NormalLaneChange::hasEnoughLengthToIntersection( bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const + Direction direction, LaneChangePaths * candidate_paths, + const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, + const bool check_safety) const { object_debug_.clear(); if (current_lanes.empty() || target_lanes.empty()) { @@ -1237,9 +1248,11 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); - if (utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + if ( + !is_stuck && + utils::lane_change::passParkedObject( + route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_, object_debug_)) { return false; } @@ -1248,7 +1261,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); + *candidate_path, target_objects, rss_params, is_stuck, object_debug_); if (is_safe) { return true; @@ -1270,8 +1283,9 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const debug_filtered_objects_ = target_objects; CollisionCheckDebugMap debug_data; + const bool is_stuck = isVehicleStuckByObstacle(current_lanes); const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->rss_params_for_abort, debug_data); + path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { // only for debug purpose object_debug_.clear(); @@ -1528,7 +1542,7 @@ bool NormalLaneChange::getAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, const LaneChangeTargetObjects & target_objects, - const utils::path_safety_checker::RSSparams & rss_params, + const utils::path_safety_checker::RSSparams & rss_params, const bool is_stuck, CollisionCheckDebugMap & debug_data) const { PathSafetyStatus path_safety_status; @@ -1552,7 +1566,6 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( auto collision_check_objects = target_objects.target_lane; const auto current_lanes = getCurrentLanes(); - const auto is_stuck = isVehicleStuckByObstacle(current_lanes); if (lane_change_parameters_->check_objects_on_current_lanes || is_stuck) { collision_check_objects.insert( From b266f47aeaf85088e02ff851e544757634ceb076 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sun, 15 Oct 2023 19:32:21 +0900 Subject: [PATCH 093/206] fix(lane_change): fix chattering for stopped objects (#5302) Update planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp fix(path_safety_checker): remove redundant parameter name Signed-off-by: Fumiya Watanabe --- .../path_safety_checker_parameters.hpp | 25 ++++---- .../path_safety_checker/safety_check.hpp | 5 +- .../src/marker_utils/utils.cpp | 5 +- .../path_safety_checker/safety_check.cpp | 60 ++++++++++++------- .../test/test_safety_check.cpp | 19 +++--- 5 files changed, 69 insertions(+), 45 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 889f016e7182f..dc113b0b0be18 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -182,18 +182,19 @@ struct SafetyCheckParams struct CollisionCheckDebug { - std::string unsafe_reason; ///< Reason indicating unsafe situation. - Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. - Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. - Pose current_obj_pose{}; ///< Detected object's current pose. - Twist object_twist{}; ///< Detected object's velocity and rotation. - Pose expected_obj_pose{}; ///< Predicted future pose of object. - double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. - double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. - double extended_polygon_lon_offset{0.0}; ///< Longitudinal offset for extended polygon. - double extended_polygon_lat_offset{0.0}; ///< Lateral offset for extended polygon. - bool is_front{false}; ///< True if object is in front of ego vehicle. - bool is_safe{false}; ///< True if situation is deemed safe. + std::string unsafe_reason; ///< Reason indicating unsafe situation. + Twist current_twist{}; ///< Ego vehicle's current velocity and rotation. + Pose expected_ego_pose{}; ///< Predicted future pose of ego vehicle. + Pose current_obj_pose{}; ///< Detected object's current pose. + Twist object_twist{}; ///< Detected object's velocity and rotation. + Pose expected_obj_pose{}; ///< Predicted future pose of object. + double rss_longitudinal{0.0}; ///< Longitudinal RSS measure. + double inter_vehicle_distance{0.0}; ///< Distance between ego vehicle and object. + double forward_lon_offset{0.0}; ///< Forward longitudinal offset for extended polygon. + double backward_lon_offset{0.0}; ///< Backward longitudinal offset for extended polygon. + double lat_offset{0.0}; ///< Lateral offset for extended polygon. + bool is_front{false}; ///< True if object is in front of ego vehicle. + bool is_safe{false}; ///< True if situation is deemed safe. std::vector ego_predicted_path; ///< ego vehicle's predicted path. std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 6e5dd2cda8920..64f5c9e5fc3e0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -66,10 +66,11 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug); + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug); + const double is_stopped_obj, CollisionCheckDebug & debug); PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution); diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index a2883403e1ccd..03ac135ec4bc1 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -641,8 +641,9 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal << "\nEgo to obj: " << info.inter_vehicle_distance << "\nExtended polygon: " << (info.is_front ? "ego" : "object") - << "\nExtended polygon lateral offset: " << info.extended_polygon_lat_offset - << "\nExtended polygon longitudinal offset: " << info.extended_polygon_lon_offset + << "\nExtended polygon lateral offset: " << info.lat_offset + << "\nExtended polygon forward longitudinal offset: " << info.forward_lon_offset + << "\nExtended polygon backward longitudinal offset: " << info.backward_lon_offset << "\nLast checked position: " << (info.is_front ? "obj in front ego" : "obj at back ego") << "\nSafe: " << (info.is_safe ? "Yes" : "No"); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index b44a3f841035d..e4698e98d46eb 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -82,28 +82,33 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin, CollisionCheckDebug & debug) + const double lon_length, const double lat_margin, const double is_stopped_obj, + CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; const double & width = vehicle_info.vehicle_width_m; const double & base_to_rear = vehicle_info.rear_overhang_m; - const double lon_offset = std::max(lon_length + base_to_front, base_to_front); - + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = base_to_front + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = + -base_to_rear - (is_stopped_obj ? lon_length / 2 : 0); // minus value const double lat_offset = width / 2.0 + lat_margin; { - debug.extended_polygon_lon_offset = lon_offset; - debug.extended_polygon_lat_offset = lat_offset; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = lat_offset; } - const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, lat_offset, 0.0); const auto p2 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, forward_lon_offset, -lat_offset, 0.0); const auto p3 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, -lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, -lat_offset, 0.0); const auto p4 = - tier4_autoware_utils::calcOffsetPose(base_link_pose, -base_to_rear, lat_offset, 0.0); + tier4_autoware_utils::calcOffsetPose(base_link_pose, backward_lon_offset, lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -118,7 +123,7 @@ Polygon2d createExtendedPolygon( Polygon2d createExtendedPolygon( const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, - CollisionCheckDebug & debug) + const double is_stopped_obj, CollisionCheckDebug & debug) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { @@ -139,19 +144,27 @@ Polygon2d createExtendedPolygon( min_y = std::min(transformed_p.y, min_y); } - const double lon_offset = max_x + lon_length; + // if stationary object, extend forward and backward by the half of lon length + const double forward_lon_offset = max_x + (is_stopped_obj ? lon_length / 2 : lon_length); + const double backward_lon_offset = min_x - (is_stopped_obj ? lon_length / 2 : 0); // minus value + const double left_lat_offset = max_y + lat_margin; const double right_lat_offset = min_y - lat_margin; { - debug.extended_polygon_lon_offset = lon_offset; - debug.extended_polygon_lat_offset = (left_lat_offset + right_lat_offset) / 2; + debug.forward_lon_offset = forward_lon_offset; + debug.backward_lon_offset = backward_lon_offset; + debug.lat_offset = (left_lat_offset + right_lat_offset) / 2; } - const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); - const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0); - const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0); - const auto p4 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, left_lat_offset, 0.0); + const auto p1 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, left_lat_offset, 0.0); + const auto p2 = + tier4_autoware_utils::calcOffsetPose(obj_pose, forward_lon_offset, right_lat_offset, 0.0); + const auto p3 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, right_lat_offset, 0.0); + const auto p4 = + tier4_autoware_utils::calcOffsetPose(obj_pose, backward_lon_offset, left_lat_offset, 0.0); Polygon2d polygon; appendPointToPolygon(polygon, p1.position); @@ -338,14 +351,17 @@ std::vector getCollidedPolygons( const auto & lon_offset = std::max(rss_dist, min_lon_length) * hysteresis_factor; const auto & lat_margin = rss_parameters.lateral_distance_max_threshold * hysteresis_factor; - const auto & extended_ego_polygon = - is_object_front - ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) - : ego_polygon; + // TODO(watanabe) fix hard coding value + const bool is_stopped_object = object_velocity < 0.3; + const auto & extended_ego_polygon = is_object_front ? createExtendedPolygon( + ego_pose, ego_vehicle_info, lon_offset, + lat_margin, is_stopped_object, debug) + : ego_polygon; const auto & extended_obj_polygon = is_object_front ? obj_polygon - : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin, debug); + : createExtendedPolygon( + obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); { debug.rss_longitudinal = rss_dist; diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 39e831bd5565b..a5fc19d1ecbbe 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -52,9 +52,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -79,9 +80,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -107,9 +109,10 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; - const auto polygon = - createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); + const auto polygon = createExtendedPolygon( + ego_pose, vehicle_info, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -155,9 +158,11 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; + const bool is_stopped_object = false; CollisionCheckDebug debug; - const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, debug); + const auto polygon = + createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, is_stopped_object, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); From 5d6449defe3309b5fc7a438eaa511e5d1985c0cf Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 15 Oct 2023 19:32:46 +0900 Subject: [PATCH 094/206] fix(lane_change): add margin in stuck detection distance (#5306) * fix(lane_change): add margin in stuck detection distance Signed-off-by: Takamasa Horibe * use margin Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- .../src/scene_module/lane_change/normal.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d155b204f62d6..e691a66d118d7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -655,7 +655,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuckByObstacle(current_lanes, max_lane_change_length)) { + if (isVehicleStuckByObstacle(current_lanes)) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is stuck. sample all longitudinal acceleration."); @@ -1668,7 +1668,21 @@ bool NormalLaneChange::isVehicleStuckByObstacle(const lanelet::ConstLanelets & c const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); - return isVehicleStuckByObstacle(current_lanes, max_lane_change_length); + const auto rss_dist = calcRssDistance( + 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + lane_change_parameters_->rss_params); + + // It is difficult to define the detection range. If it is too short, the stuck will not be + // determined, even though you are stuck by an obstacle. If it is too long, + // the ego will be judged to be stuck by a distant vehicle, even though the ego is only + // stopped at a traffic light. Essentially, the calculation should be based on the information of + // the stop reason, but this is outside the scope of one module. I keep it as a TODO. + constexpr double DETECTION_DISTANCE_MARGIN = 10.0; + const auto detection_distance = max_lane_change_length + rss_dist + + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; + RCLCPP_INFO(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + + return isVehicleStuckByObstacle(current_lanes, detection_distance); } void NormalLaneChange::setStopPose(const Pose & stop_pose) From 95e8f212a6a8b5eb92941ae0d4d8dc997970de25 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 16 Oct 2023 12:46:17 +0900 Subject: [PATCH 095/206] refactor(lane_change): add debug log (#5308) Signed-off-by: Takamasa Horibe --- .../config/logger_config.yaml | 4 + .../scene_module/lane_change/base_class.hpp | 3 + .../scene_module/lane_change/normal.hpp | 1 - .../src/scene_module/lane_change/normal.cpp | 85 ++++++++++++++----- 4 files changed, 72 insertions(+), 21 deletions(-) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index da5cc757682c5..97c3104242026 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -14,6 +14,10 @@ behavior_path_planner_avoidance: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance +behavior_path_planner_lane_change: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: lane_change + behavior_velocity_planner: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 26f68e69b3728..472564a061f04 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -271,6 +271,9 @@ class LaneChangeBase mutable LaneChangeTargetObjects debug_filtered_objects_{}; mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; + + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change"); + mutable rclcpp::Clock clock_{RCL_ROS_TIME}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index e9e361e1c39b8..ccadcd7144a9b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -173,7 +173,6 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } - rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); double stop_time_{0.0}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index e691a66d118d7..fadc63dd8834e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -642,6 +642,8 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( // if max acc is not positive, then we do the normal sampling if (max_acc <= 0.0) { + RCLCPP_DEBUG( + logger_, "Available max acc <= 0. Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -650,6 +652,9 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( const double max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); if (max_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { + RCLCPP_DEBUG( + logger_, "No enough distance to the end of lane. Normal sampling for acc: [%f ~ %f]", min_acc, + max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -658,7 +663,8 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( if (isVehicleStuckByObstacle(current_lanes)) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( - logger_, clock, 1000, "Vehicle is stuck. sample all longitudinal acceleration."); + logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, + max_acc); return utils::lane_change::getAccelerationValues( min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -668,12 +674,17 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( if (route_handler.isInGoalRouteSection(target_lanes.back())) { const auto goal_pose = route_handler.getGoalPose(); if (max_lane_change_length < utils::getSignedDistance(current_pose, goal_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to goal has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } } else if (max_lane_change_length < utils::getDistanceToEndOfLane(current_pose, target_lanes)) { + RCLCPP_DEBUG( + logger_, "Distance to end of lane has enough distance. Sample only max_acc: %f", max_acc); return {max_acc}; } + RCLCPP_DEBUG(logger_, "Normal sampling for acc: [%f ~ %f]", min_acc, max_acc); return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } @@ -1021,6 +1032,7 @@ bool NormalLaneChange::getLaneChangePaths( { object_debug_.clear(); if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } const auto & route_handler = *getRouteHandler(); @@ -1056,6 +1068,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_neighbor_preferred_lane_poly_2d = utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } @@ -1067,8 +1080,18 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->reserve( longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); + RCLCPP_DEBUG( + logger_, "lane change sampling start. Sampling num for prep_time: %lu, acc: %lu", + prepare_durations.size(), longitudinal_acc_sampling_values.size()); + for (const auto & prepare_duration : prepare_durations) { for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration + << ", lon_acc = " << sampled_longitudinal_acc); + }; + // get path on original lanes const auto prepare_velocity = std::max( current_velocity + sampled_longitudinal_acc * prepare_duration, @@ -1086,7 +1109,7 @@ bool NormalLaneChange::getLaneChangePaths( auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); + debug_print("prepare segment is empty...? Unexpected."); continue; } @@ -1097,8 +1120,7 @@ bool NormalLaneChange::getLaneChangePaths( // Check if the lane changing start point is not on the lanes next to target lanes, if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + debug_print("lane change start getEgoPose() is behind target lanelet!"); break; } @@ -1113,11 +1135,21 @@ bool NormalLaneChange::getLaneChangePaths( common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - constexpr double lateral_acc_epsilon = 0.01; - for (double lateral_acc = min_lateral_acc; - lateral_acc < max_lateral_acc + lateral_acc_epsilon; - lateral_acc += lateral_acc_resolution) { + std::vector sample_lat_acc; + constexpr double eps = 0.01; + for (double a = min_lateral_acc; a < max_lateral_acc + eps; a += lateral_acc_resolution) { + sample_lat_acc.push_back(a); + } + RCLCPP_DEBUG(logger_, " - sampling num for lat_acc: %lu", sample_lat_acc.size()); + + for (const auto & lateral_acc : sample_lat_acc) { + const auto debug_print = [&](const auto & s) { + RCLCPP_DEBUG_STREAM( + logger_, " - " << s << " : prep_time = " << prepare_duration << ", lon_acc = " + << sampled_longitudinal_acc << ", lat_acc = " << lateral_acc); + }; + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); const double longitudinal_acc_on_lane_changing = @@ -1133,7 +1165,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + debug_print("Reject: length of lane changing path is longer than length to goal!!"); continue; } @@ -1151,7 +1183,7 @@ bool NormalLaneChange::getLaneChangePaths( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > s_goal) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + debug_print("Reject: length of lane changing path is longer than length to goal!!"); continue; } } @@ -1161,7 +1193,7 @@ bool NormalLaneChange::getLaneChangePaths( initial_lane_changing_velocity, next_lane_change_buffer); if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); + debug_print("Reject: target segment is empty!! something wrong..."); continue; } @@ -1178,7 +1210,9 @@ bool NormalLaneChange::getLaneChangePaths( boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); if (!is_valid_start_point) { - // lane changing points are not inside of the target preferred lanes or its neighbors + debug_print( + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); continue; } @@ -1190,7 +1224,7 @@ bool NormalLaneChange::getLaneChangePaths( next_lane_change_buffer); if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); + debug_print("Reject: target_lane_reference_path is empty!!"); continue; } @@ -1215,32 +1249,31 @@ bool NormalLaneChange::getLaneChangePaths( sorted_lane_ids); if (!candidate_path) { - RCLCPP_DEBUG(logger_, "no candidate path!!"); + debug_print("Reject: failed to generate candidate path!!"); continue; } if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { - RCLCPP_DEBUG(logger_, "invalid candidate path!!"); + debug_print("Reject: invalid candidate path!!"); continue; } if ( lane_change_parameters_->regulate_on_crosswalk && !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { - RCLCPP_DEBUG(logger_, "Including crosswalk!!"); if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including crosswalk!!"); continue; } - auto clock{rclcpp::Clock{RCL_ROS_TIME}}; - RCLCPP_WARN_THROTTLE( - logger_, clock, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); + RCLCPP_INFO_THROTTLE( + logger_, clock_, 1000, "Stop time is over threshold. Allow lane change in crosswalk."); } if ( lane_change_parameters_->regulate_on_intersection && !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { - RCLCPP_DEBUG(logger_, "Including intersection!!"); if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + debug_print("Reject: including intersection!!"); continue; } RCLCPP_WARN_STREAM( @@ -1253,10 +1286,14 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::passParkedObject( route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, is_goal_in_route, *lane_change_parameters_, object_debug_)) { + debug_print( + "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " + "lane change."); return false; } if (!check_safety) { + debug_print("ACCEPT!!!: it is valid (and safety check is skipped)."); return false; } @@ -1264,12 +1301,16 @@ bool NormalLaneChange::getLaneChangePaths( *candidate_path, target_objects, rss_params, is_stuck, object_debug_); if (is_safe) { + debug_print("ACCEPT!!!: it is valid and safe!"); return true; } + + debug_print("Reject: sampled path is not safe."); } } } + RCLCPP_DEBUG(logger_, "No safety path found."); return false; } @@ -1630,11 +1671,13 @@ bool NormalLaneChange::isVehicleStuckByObstacle( { // Ego is still moving, not in stuck if (std::abs(getEgoVelocity()) > lane_change_parameters_->stop_velocity_threshold) { + RCLCPP_DEBUG(logger_, "Ego is still moving, not in stuck"); return false; } // Ego is just stopped, not sure it is in stuck yet. if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + RCLCPP_DEBUG(logger_, "Ego is just stopped, counting for stuck judge... (%f)", getStopTime()); return false; } @@ -1652,11 +1695,13 @@ bool NormalLaneChange::isVehicleStuckByObstacle( const auto ego_to_obj_dist = getArcCoordinates(current_lanes, p).length - base_distance; if (0 < ego_to_obj_dist && ego_to_obj_dist < obstacle_check_distance) { + RCLCPP_DEBUG(logger_, "Stationary object is in front of ego."); return true; // Stationary object is in front of ego. } } // No stationary objects found in obstacle_check_distance + RCLCPP_DEBUG(logger_, "No stationary objects found in obstacle_check_distance"); return false; } From 5725c2daf9b6d00a5f52abe028641af7c71dc192 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Mon, 16 Oct 2023 12:59:34 +0900 Subject: [PATCH 096/206] chore(ground_segmentation): update docs (#4806) * chore(ground_segmentation): update docs Signed-off-by: badai-nguyen * chore: update figure Signed-off-by: badai-nguyen * style(pre-commit): autofix --------- Signed-off-by: badai-nguyen Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scan_ground_filter_parameters.drawio.svg | 225 ++++++++++++++++++ .../docs/scan-ground-filter.md | 39 +-- 2 files changed, 245 insertions(+), 19 deletions(-) create mode 100644 perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg diff --git a/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg b/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg new file mode 100644 index 0000000000000..985b99d82ba9b --- /dev/null +++ b/perception/ground_segmentation/docs/image/scan_ground_filter_parameters.drawio.svg @@ -0,0 +1,225 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ detection_range_z_max +
+
+
+
+ + detection_range_z_max + + + + + + + + + + + +
+
+
+
+ global_slope +
+
+
+
+ + global_slope + + + + + + + + + + + + +
+
+
+
+ local_slope +
+
+
+
+ + local_slope + + + + + + + + + + + +
+
+
+
+
split_height_distance
+
+
+
+
+ + split_height_distance + + + + + + + + + +
+
+
+
+
+
grid_size_m
+
+
+
+
+
+ + grid_size_m + + + + + + + + + + + + +
+
+
+
+ z +
+
+
+
+ + z + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/ground_segmentation/docs/scan-ground-filter.md b/perception/ground_segmentation/docs/scan-ground-filter.md index 0c07ce600f625..bc44544fa298c 100644 --- a/perception/ground_segmentation/docs/scan-ground-filter.md +++ b/perception/ground_segmentation/docs/scan-ground-filter.md @@ -29,25 +29,26 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref #### Core Parameters -| Name | Type | Default Value | Description | -| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------- | -| `input_frame` | string | "base_link" | frame id of input pointcloud | -| `output_frame` | string | "base_link" | frame id of output pointcloud | -| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg] | -| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] | -| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | -| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to to distinguishing far and near [m] | -| `split_height_distance` | double | 0.2 | The height threshold to distinguishing far and near [m] | -| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | -| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | -| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | -| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m], applied only for elevation_grid_mode | -| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | -| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode | -| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | -| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | -| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | -| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | +![scan_ground_parameter](./image/scan_ground_filter_parameters.drawio.svg) +| Name | Type | Default Value | Description | +| --------------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `input_frame` | string | "base_link" | frame id of input pointcloud | +| `output_frame` | string | "base_link" | frame id of output pointcloud | +| `global_slope_max_angle_deg` | double | 8.0 | The global angle to classify as the ground or object [deg].
A large threshold may reduce false positive of high slope road classification but it may lead to increase false negative of non-ground classification, particularly for small objects. | +| `local_slope_max_angle_deg` | double | 10.0 | The local angle to classify as the ground or object [deg] when comparing with adjacent point.
A small value enhance accuracy classification of object with inclined surface. This should be considered together with `split_points_distance_tolerance` value. | +| `radial_divider_angle_deg` | double | 1.0 | The angle which divide the whole pointcloud to sliced group [deg] | +| `split_points_distance_tolerance` | double | 0.2 | The xy-distance threshold to distinguish far and near [m] | +| `split_height_distance` | double | 0.2 | The height threshold to distinguish ground and non-ground pointcloud when comparing with adjacent points [m].
A small threshold improves classification of non-ground point, especially for high elevation resolution pointcloud lidar. However, it might cause false positive for small step-like road surface or misaligned multiple lidar configuration. | +| `use_virtual_ground_point` | bool | true | whether to use the ground center of front wheels as the virtual ground point. | +| `detection_range_z_max` | float | 2.5 | Maximum height of detection range [m], applied only for elevation_grid_mode | +| `center_pcl_shift` | float | 0.0 | The x-axis offset of addition LiDARs from vehicle center of mass [m],
recommended to use only for additional LiDARs in elevation_grid_mode | +| `non_ground_height_threshold` | float | 0.2 | Height threshold of non ground objects [m] as `split_height_distance` and applied only for elevation_grid_mode | +| `grid_mode_switch_radius` | float | 20.0 | The distance where grid division mode change from by distance to by vertical angle [m],
applied only for elevation_grid_mode | +| `grid_size_m` | float | 0.5 | The first grid size [m], applied only for elevation_grid_mode.
A large value enhances the prediction stability for ground surface. suitable for rough surface or multiple lidar configuration. | +| `gnd_grid_buffer_size` | uint16 | 4 | Number of grids using to estimate local ground slope,
applied only for elevation_grid_mode | +| `low_priority_region_x` | float | -20.0 | The non-zero x threshold in back side from which small objects detection is low priority [m] | +| `elevation_grid_mode` | bool | true | Elevation grid scan mode option | +| `use_recheck_ground_cluster` | bool | true | Enable recheck ground cluster | ## Assumptions / Known limits From 35cf82eefa96326b5d6f9754405387299ea7a752 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 16 Oct 2023 13:54:43 +0900 Subject: [PATCH 097/206] fix(lane_change): change logger level in isVehicleStuckByObstacle (#5315) Signed-off-by: Zulfaqar Azmi --- .../src/scene_module/lane_change/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index fadc63dd8834e..12b6d53a83220 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -1725,7 +1725,7 @@ bool NormalLaneChange::isVehicleStuckByObstacle(const lanelet::ConstLanelets & c constexpr double DETECTION_DISTANCE_MARGIN = 10.0; const auto detection_distance = max_lane_change_length + rss_dist + getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; - RCLCPP_INFO(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); + RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); return isVehicleStuckByObstacle(current_lanes, detection_distance); } From dbc6de6f15507f6132247e4a875a1404db814bc3 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 16 Oct 2023 17:23:15 +0900 Subject: [PATCH 098/206] feat(behavior_path_planner): curvature based drivable area expansion (#5294) * [DEBUG] Launch planner with konsole + gdb Signed-off-by: Maxime CLEMENT * Switch to new curvature based dynamic drivable area expansion Signed-off-by: Maxime CLEMENT * Cleanup + remove the old code Signed-off-by: Maxime CLEMENT * Handle uncrossable lines/polygons (may not be accurate enough) Signed-off-by: Maxime CLEMENT * Add runtime measurements Signed-off-by: Maxime CLEMENT * [WIP] Reuse previously calculated raw curvatures Signed-off-by: Maxime CLEMENT * Fix bug with lateral offset distance and repeating path points Signed-off-by: Maxime CLEMENT * Remove self crossings in the expanded bounds Signed-off-by: Maxime CLEMENT * Big cleanup + update parameters Signed-off-by: Maxime CLEMENT * Revert "[DEBUG] Launch planner with konsole + gdb" This reverts commit e8f04def14e566a7f5e53d5562bef5fc85628649. Signed-off-by: Maxime CLEMENT * Remove svg debug output Signed-off-by: Maxime CLEMENT * Update parameter file Signed-off-by: Maxime CLEMENT * Add parameter to enable/disable printing the runtime Signed-off-by: Maxime CLEMENT * Fix append of new path points to satisfy the resampling interval Signed-off-by: Maxime CLEMENT * Add smoothing.extra_arc_length param Signed-off-by: Maxime CLEMENT * Code cleanup + add docstrings Signed-off-by: Maxime CLEMENT * Fix spellcheck Signed-off-by: Maxime CLEMENT * Fix initial path poses (no longer cropped) and fix test Signed-off-by: Maxime CLEMENT --------- Signed-off-by: Maxime CLEMENT --- planning/behavior_path_planner/CMakeLists.txt | 1 - .../config/drivable_area_expansion.param.yaml | 28 +- .../behavior_path_planner/data_manager.hpp | 3 +- .../drivable_area_expansion.hpp | 84 ++- .../drivable_area_expansion/expansion.hpp | 85 --- .../drivable_area_expansion/footprints.hpp | 13 +- .../drivable_area_expansion/map_utils.hpp | 13 +- .../drivable_area_expansion/parameters.hpp | 74 ++- .../path_projection.hpp | 36 +- .../utils/drivable_area_expansion/types.hpp | 22 +- .../src/behavior_path_planner_node.cpp | 36 +- .../drivable_area_expansion.cpp | 534 +++++++++--------- .../drivable_area_expansion/expansion.cpp | 237 -------- .../drivable_area_expansion/footprints.cpp | 46 +- .../drivable_area_expansion/map_utils.cpp | 25 +- .../behavior_path_planner/src/utils/utils.cpp | 2 +- .../test/test_drivable_area_expansion.cpp | 221 ++------ 17 files changed, 525 insertions(+), 935 deletions(-) delete mode 100644 planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp delete mode 100644 planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f62e418371401..ee6e50f5a9553 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -50,7 +50,6 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/drivable_area_expansion/drivable_area_expansion.cpp src/utils/drivable_area_expansion/map_utils.cpp src/utils/drivable_area_expansion/footprints.cpp - src/utils/drivable_area_expansion/expansion.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp src/marker_utils/utils.cpp diff --git a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml index 160ebdc180020..c0b6f259c7726 100644 --- a/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml +++ b/planning/behavior_path_planner/config/drivable_area_expansion.param.yaml @@ -5,15 +5,19 @@ drivable_area_left_bound_offset: 0.0 drivable_area_types_to_skip: [road_border] - # Dynamic expansion by projecting the ego footprint along the path + # Dynamic expansion by using the path curvature dynamic_expansion: - enabled: false + enabled: true + print_runtime: false + max_expansion_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) + smoothing: + curvature_average_window: 3 # window size used for smoothing the curvatures using a moving window average + max_bound_rate: 1.0 # [m/m] maximum rate of change of the bound lateral distance over its arc length + extra_arc_length: 2.0 # [m] extra arc length where an expansion distance is initially applied ego: - extra_footprint_offset: - front: 0.5 # [m] extra length to add to the front of the ego footprint - rear: 0.5 # [m] extra length to add to the rear of the ego footprint - left: 0.5 # [m] extra length to add to the left of the ego footprint - right: 0.5 # [m] extra length to add to the rear of the ego footprint + extra_wheelbase: 0.0 # [m] extra length to add to the wheelbase + extra_front_overhang: 0.5 # [m] extra length to add to the front overhang + extra_width: 1.0 # [m] extra length to add to the width dynamic_objects: avoid: true # if true, the drivable area is not expanded in the predicted path of dynamic objects extra_footprint_offset: @@ -24,16 +28,8 @@ path_preprocessing: max_arc_length: 50.0 # [m] maximum arc length along the path where the ego footprint is projected (0.0 means no limit) resample_interval: 2.0 # [m] fixed interval between resampled path points (0.0 means path points are directly used) - expansion: - method: polygon # method used to expand the drivable area. Either 'lanelet' or 'polygon'. - # 'lanelet': add lanelets overlapped by the ego footprints - # 'polygon': add polygons built around sections of the ego footprint that go out of the drivable area - max_distance: 0.0 # [m] maximum distance by which the drivable area can be expended (0.0 means no limit) - extra_arc_length: 0.5 # [m] extra expansion arc length around an ego footprint + reuse_max_deviation: 0.5 # [m] if the path changes by more than this value, the curvatures are recalculated. Otherwise they are reused. avoid_linestring: types: # linestring types in the lanelet maps that will not be crossed when expanding the drivable area - road_border distance: 0.0 # [m] distance to keep between the drivable area and the linestrings to avoid - compensate: - enable: true # if true, when the drivable area cannot be expanded in one direction to completely include the ego footprint, it is expanded in the opposite direction - extra_distance: 3.0 # [m] extra distance to add to the compensation diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 7f1648c463097..9280a81e643ca 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -148,7 +148,8 @@ struct PlannerData BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; - mutable std::optional drivable_area_expansion_prev_crop_pose; + mutable std::vector drivable_area_expansion_prev_path_poses{}; + mutable std::vector drivable_area_expansion_prev_curvatures{}; mutable TurnSignalDecider turn_signal_decider; TurnIndicatorsCommand getTurnSignal( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 8293e0a1d10a9..19ea89a3ce3c7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -24,30 +24,76 @@ #include #include +#include namespace drivable_area_expansion { -/// @brief Expand the drivable area based on the projected ego footprint along the path +/// @brief Expand the drivable area based on the path curvature and the vehicle dimensions /// @param[inout] path path whose drivable area will be expanded -/// @param[inout] planner_data planning data (params, dynamic objects, route handler, ...) -/// @param[in] path_lanes lanelets of the path -void expandDrivableArea( +/// @param[inout] planner_data planning data (params, dynamic objects, vehicle info, ...) +void expand_drivable_area( PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes); - -/// @brief Create a polygon combining the drivable area of a path and some expansion polygons -/// @param[in] path path and its drivable area -/// @param[in] expansion_polygons polygons to add to the drivable area -/// @return expanded drivable area polygon -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons); - -/// @brief Update the drivable area of the given path with the given polygon -/// @details this function splits the polygon into a left and right bound and sets it in the path -/// @param[in] path path whose drivable area will be expanded -/// @param[in] expanded_drivable_area polygon of the new drivable area -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area); + const std::shared_ptr planner_data); + +/// @brief prepare path poses and try to reuse their previously calculated curvatures +/// @details poses are reused if they do not deviate too much from the current path +/// @param [in] path input path +/// @param [inout] prev_poses previous poses to reuse +/// @param [inout] prev_curvatures previous curvatures to reuse +/// @param [in] ego_point current ego point +/// @param [in] params parameters for reuse criteria and resampling interval +void update_path_poses_and_previous_curvatures( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params); + +/// @brief calculate the minimum lane width based on the path curvature and the vehicle dimensions +/// @cite Lim, H., Kim, C., and Jo, A., "Model Predictive Control-Based Lateral Control of +/// Autonomous Large-Size Bus on Road with Large Curvature," SAE Technical Paper 2021-01-0099, 2021, +/// https://doi.org/10.4271/2021-01-0099 +/// @param [in] curvature curvature +/// @param [in] params parameters containing the vehicle dimensions +/// @return minimum lane width +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params); + +/// @brief smooth the bound by applying a limit on its rate of change +/// @details rate of change is the lateral distance from the path over the arc length along the path +/// @param [inout] bound_distances bound distances (lateral distance from the path) +/// @param [in] bound_points bound points +/// @param [in] max_rate [m/m] maximum rate of lateral deviation over arc length +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate); + +/// @brief calculate the maximum distance by which a bound can be expanded +/// @param [in] path_poses input path +/// @param [in] bound bound points +/// @param [in] uncrossable_lines lines that limit the bound expansion +/// @param [in] uncrossable_polygons polygons that limit the bound expansion +/// @param [in] params parameters with the buffer distance to keep with lines, +/// and the static maximum expansion distance +std::vector calculate_maximum_distance( + const std::vector & path_poses, const std::vector bound, + const std::vector & uncrossable_lines, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params); + +/// @brief expand a bound by the given lateral distances away from the path +/// @param [inout] bound bound points to expand +/// @param [in] path_poses input path +/// @param [in] distances new lateral distances of the bound points away from the path +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & distances); + +/// @brief calculate smoothed curvatures +/// @details smoothing is done using a moving average +/// @param [in] poses input poses used to calculate the curvatures +/// @param [in] smoothing_window_size size of the window used for the moving average +/// @return smoothed curvatures of the input poses +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size); + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp deleted file mode 100644 index 70cc8a8bc5925..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ - -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - -#include - -#include - -#include -#include - -namespace drivable_area_expansion -{ -/// @brief Calculate the distance limit required for the polygon to not cross the limit lines -/// @details Calculate the minimum distance from base_ls to an intersection of limit_lines and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_lines lines we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines); - -/// @brief Calculate the distance limit required for the polygon to not cross the limit polygons. -/// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and -/// expansion_polygon -/// @param[in] base_ls base linestring from which the distance is calculated -/// @param[in] expansion_polygon polygon to consider -/// @param[in] limit_polygons polygons we do not want to cross -/// @return distance limit -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons); - -/// @brief Create a polygon from a base line with a given expansion distance -/// @param[in] base_ls base linestring from which the polygon is created -/// @param[in] dist desired expansion distance from the base line -/// @param[in] is_left_side desired side of the expansion from the base line -/// @return expansion polygon -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path path and its drivable area -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params); - -/// @brief Create polygons for the area where the drivable area should be expanded -/// @param[in] path_lanes lanelets of the current path -/// @param[in] route_handler route handler -/// @param[in] path_footprints polygons of the ego footprint projected along the path -/// @param[in] predicted_paths polygons of the dynamic objects' predicted paths -/// @param[in] params expansion parameters -/// @return expansion polygons -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params); -} // namespace drivable_area_expansion - -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__EXPANSION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 8fc0157710dc8..418a9a5a68572 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -43,27 +43,20 @@ namespace drivable_area_expansion /// @param[in] x translation distance on the x axis /// @param[in] y translation distance on the y axis /// @return translated polygon -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y); +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y); /// @brief create the footprint of a pose and its base footprint /// @param[in] pose the origin pose of the footprint /// @param[in] base_footprint the base axis-aligned footprint /// @return footprint polygon -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint); +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint); /// @brief create footprints of the predicted paths of an object /// @param [in] objects objects from which to create polygons /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); - -/// @brief create the footprint polygon from a path -/// @param[in] path the path for which to create a footprint -/// @param[in] params expansion parameters defining how to create the footprint -/// @return footprint polygons of the path -multi_polygon_t createPathFootprints( - const std::vector & path, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 4524bd2be2299..6f96b83237310 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -15,6 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -24,18 +25,20 @@ namespace drivable_area_expansion { -/// @brief Extract uncrossable linestrings from the lanelet map +/// @brief Extract uncrossable linestrings from the lanelet map that are in range of ego /// @param[in] lanelet_map lanelet map -/// @param[in] uncrossable_types types that cannot be crossed +/// @param[in] ego_point point of the current ego position +/// @param[in] params parameters with linestring types that cannot be crossed and maximum range /// @return the uncrossable linestrings -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types); +MultiLineString2d extract_uncrossable_lines( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params); /// @brief Determine if the given linestring has one of the given types /// @param[in] ls linestring to check /// @param[in] types type strings to check /// @return true if the linestring has one of the given types -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types); +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index 81eab9560b736..e7275960b0888 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -32,12 +32,9 @@ struct DrivableAreaExpansionParameters static constexpr auto DRIVABLE_AREA_LEFT_BOUND_OFFSET_PARAM = "drivable_area_left_bound_offset"; static constexpr auto DRIVABLE_AREA_TYPES_TO_SKIP_PARAM = "drivable_area_types_to_skip"; static constexpr auto ENABLED_PARAM = "dynamic_expansion.enabled"; - static constexpr auto EGO_EXTRA_OFFSET_FRONT = - "dynamic_expansion.ego.extra_footprint_offset.front"; - static constexpr auto EGO_EXTRA_OFFSET_REAR = "dynamic_expansion.ego.extra_footprint_offset.rear"; - static constexpr auto EGO_EXTRA_OFFSET_LEFT = "dynamic_expansion.ego.extra_footprint_offset.left"; - static constexpr auto EGO_EXTRA_OFFSET_RIGHT = - "dynamic_expansion.ego.extra_footprint_offset.right"; + static constexpr auto EGO_EXTRA_FRONT_OVERHANG = "dynamic_expansion.ego.extra_front_overhang"; + static constexpr auto EGO_EXTRA_WHEELBASE = "dynamic_expansion.ego.extra_wheelbase"; + static constexpr auto EGO_EXTRA_WIDTH = "dynamic_expansion.ego.extra_width"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_FRONT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.front"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_REAR = @@ -46,34 +43,36 @@ struct DrivableAreaExpansionParameters "dynamic_expansion.dynamic_objects.extra_footprint_offset.left"; static constexpr auto DYN_OBJECTS_EXTRA_OFFSET_RIGHT = "dynamic_expansion.dynamic_objects.extra_footprint_offset.right"; - static constexpr auto EXPANSION_METHOD_PARAM = "dynamic_expansion.expansion.method"; - static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.expansion.max_distance"; + static constexpr auto MAX_EXP_DIST_PARAM = "dynamic_expansion.max_expansion_distance"; static constexpr auto RESAMPLE_INTERVAL_PARAM = "dynamic_expansion.path_preprocessing.resample_interval"; static constexpr auto MAX_PATH_ARC_LENGTH_PARAM = "dynamic_expansion.path_preprocessing.max_arc_length"; - static constexpr auto EXTRA_ARC_LENGTH_PARAM = "dynamic_expansion.expansion.extra_arc_length"; + static constexpr auto MAX_REUSE_DEVIATION_PARAM = + "dynamic_expansion.path_preprocessing.reuse_max_deviation"; static constexpr auto AVOID_DYN_OBJECTS_PARAM = "dynamic_expansion.dynamic_objects.avoid"; static constexpr auto AVOID_LINESTRING_TYPES_PARAM = "dynamic_expansion.avoid_linestring.types"; static constexpr auto AVOID_LINESTRING_DIST_PARAM = "dynamic_expansion.avoid_linestring.distance"; - static constexpr auto COMPENSATE_PARAM = "dynamic_expansion.avoid_linestring.compensate.enable"; - static constexpr auto EXTRA_COMPENSATE_PARAM = - "dynamic_expansion.avoid_linestring.compensate.extra_distance"; + static constexpr auto SMOOTHING_CURVATURE_WINDOW_PARAM = + "dynamic_expansion.smoothing.curvature_average_window"; + static constexpr auto SMOOTHING_MAX_BOUND_RATE_PARAM = + "dynamic_expansion.smoothing.max_bound_rate"; + static constexpr auto SMOOTHING_EXTRA_ARC_LENGTH_PARAM = + "dynamic_expansion.smoothing.extra_arc_length"; + static constexpr auto PRINT_RUNTIME_PARAM = "dynamic_expansion.print_runtime"; - double drivable_area_right_bound_offset; - double drivable_area_left_bound_offset; - std::vector drivable_area_types_to_skip; + // static expansion + double drivable_area_right_bound_offset{}; + double drivable_area_left_bound_offset{}; + std::vector drivable_area_types_to_skip{}; + // dynamic expansion bool enabled = false; - std::string expansion_method{}; double avoid_linestring_dist{}; - double ego_left_offset{}; - double ego_right_offset{}; - double ego_rear_offset{}; - double ego_front_offset{}; - double ego_extra_left_offset{}; - double ego_extra_right_offset{}; - double ego_extra_rear_offset{}; - double ego_extra_front_offset{}; + double extra_front_overhang{}; + double extra_wheelbase{}; + double extra_width{}; + int curvature_average_window{}; + double max_bound_rate{}; double dynamic_objects_extra_left_offset{}; double dynamic_objects_extra_right_offset{}; double dynamic_objects_extra_rear_offset{}; @@ -82,10 +81,11 @@ struct DrivableAreaExpansionParameters double max_path_arc_length{}; double resample_interval{}; double extra_arc_length{}; + double max_reuse_deviation{}; bool avoid_dynamic_objects{}; + bool print_runtime{}; std::vector avoid_linestring_types{}; - bool compensate_uncrossable_lines = false; - double compensate_extra_dist{}; + vehicle_info_util::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -100,12 +100,15 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(DRIVABLE_AREA_TYPES_TO_SKIP_PARAM); enabled = node.declare_parameter(ENABLED_PARAM); max_expansion_distance = node.declare_parameter(MAX_EXP_DIST_PARAM); + extra_front_overhang = node.declare_parameter(EGO_EXTRA_FRONT_OVERHANG); + extra_wheelbase = node.declare_parameter(EGO_EXTRA_WHEELBASE); + extra_width = node.declare_parameter(EGO_EXTRA_WIDTH); + curvature_average_window = node.declare_parameter(SMOOTHING_CURVATURE_WINDOW_PARAM); + max_bound_rate = node.declare_parameter(SMOOTHING_MAX_BOUND_RATE_PARAM); + extra_arc_length = node.declare_parameter(SMOOTHING_EXTRA_ARC_LENGTH_PARAM); max_path_arc_length = node.declare_parameter(MAX_PATH_ARC_LENGTH_PARAM); resample_interval = node.declare_parameter(RESAMPLE_INTERVAL_PARAM); - ego_extra_front_offset = node.declare_parameter(EGO_EXTRA_OFFSET_FRONT); - ego_extra_rear_offset = node.declare_parameter(EGO_EXTRA_OFFSET_REAR); - ego_extra_left_offset = node.declare_parameter(EGO_EXTRA_OFFSET_LEFT); - ego_extra_right_offset = node.declare_parameter(EGO_EXTRA_OFFSET_RIGHT); + max_reuse_deviation = node.declare_parameter(MAX_REUSE_DEVIATION_PARAM); dynamic_objects_extra_front_offset = node.declare_parameter(DYN_OBJECTS_EXTRA_OFFSET_FRONT); dynamic_objects_extra_rear_offset = @@ -118,16 +121,9 @@ struct DrivableAreaExpansionParameters node.declare_parameter>(AVOID_LINESTRING_TYPES_PARAM); avoid_dynamic_objects = node.declare_parameter(AVOID_DYN_OBJECTS_PARAM); avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); - extra_arc_length = node.declare_parameter(EXTRA_ARC_LENGTH_PARAM); - compensate_uncrossable_lines = node.declare_parameter(COMPENSATE_PARAM); - compensate_extra_dist = node.declare_parameter(EXTRA_COMPENSATE_PARAM); - expansion_method = node.declare_parameter(EXPANSION_METHOD_PARAM); + print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - ego_left_offset = vehicle_info.max_lateral_offset_m; - ego_right_offset = vehicle_info.min_lateral_offset_m; - ego_rear_offset = vehicle_info.min_longitudinal_offset_m; - ego_front_offset = vehicle_info.max_longitudinal_offset_m; + vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); } }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp index 3e2b177f59167..93afaad825582 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp @@ -33,10 +33,10 @@ namespace drivable_area_expansion /// @param p2 second segment point /// @return projected point and corresponding distance inline PointDistance point_to_segment_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; + const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; @@ -48,7 +48,7 @@ inline PointDistance point_to_segment_projection( if (c2 <= c1) return {p2, boost::geometry::distance(p, p2) * dist_sign}; const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -59,10 +59,10 @@ inline PointDistance point_to_segment_projection( /// @param p2 second line point /// @return projected point and corresponding distance inline PointDistance point_to_line_projection( - const point_t & p, const point_t & p1, const point_t & p2) + const Point2d & p, const Point2d & p1, const Point2d & p2) { - const point_t p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; - const point_t p_vec = {p.x() - p1.x(), p.y() - p1.y()}; + const Point2d p2_vec = {p2.x() - p1.x(), p2.y() - p1.y()}; + const Point2d p_vec = {p.x() - p1.x(), p.y() - p1.y()}; const auto cross = p2_vec.x() * p_vec.y() - p2_vec.y() * p_vec.x(); const auto dist_sign = cross < 0.0 ? -1.0 : 1.0; @@ -70,7 +70,7 @@ inline PointDistance point_to_line_projection( const auto c1 = boost::geometry::dot_product(p_vec, p2_vec); const auto c2 = boost::geometry::dot_product(p2_vec, p2_vec); const auto projection = p1 + (p2_vec * c1 / c2); - const auto projection_point = point_t{projection.x(), projection.y()}; + const auto projection_point = Point2d{projection.x(), projection.y()}; return {projection_point, boost::geometry::distance(p, projection_point) * dist_sign}; } @@ -78,7 +78,7 @@ inline PointDistance point_to_line_projection( /// @param p point to project /// @param ls linestring /// @return projected point, corresponding distance, and arc length along the linestring -inline Projection point_to_linestring_projection(const point_t & p, const linestring_t & ls) +inline Projection point_to_linestring_projection(const Point2d & p, const LineString2d & ls) { Projection closest; closest.distance = std::numeric_limits::max(); @@ -100,14 +100,14 @@ inline Projection point_to_linestring_projection(const point_t & p, const linest /// @param p2 second vector point /// @param dist distance /// @return point p such that (p1,p) is orthogonal to (p1,p2) at the given distance -inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const double dist) +inline Point2d normal_at_distance(const Point2d & p1, const Point2d & p2, const double dist) { auto base = p1; auto normal_vector = p2; boost::geometry::subtract_point(normal_vector, base); boost::geometry::detail::vec_normalize(normal_vector); boost::geometry::multiply_value(normal_vector, dist); - return point_t{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; + return Point2d{base.x() - normal_vector.y(), base.y() + normal_vector.x()}; } /// @brief interpolate between two points @@ -115,7 +115,7 @@ inline point_t normal_at_distance(const point_t & p1, const point_t & p2, const /// @param b second point /// @param ratio interpolation ratio such that 0 yields a, and 1 yields b /// @return point interpolated between a and b as per the given ratio -inline point_t lerp_point(const point_t & a, const point_t & b, const double ratio) +inline Point2d lerp_point(const Point2d & a, const Point2d & b, const double ratio) { return {interpolation::lerp(a.x(), b.x(), ratio), interpolation::lerp(a.y(), b.y(), ratio)}; } @@ -125,10 +125,10 @@ inline point_t lerp_point(const point_t & a, const point_t & b, const double rat /// @param arc_length arc length along the reference linestring of the resulting point /// @param distance distance from the reference linestring of the resulting point /// @return point at the distance and arc length relative to the reference linestring -inline segment_t linestring_to_point_projection( - const linestring_t & ls, const double arc_length, const double distance) +inline Segment2d linestring_to_point_projection( + const LineString2d & ls, const double arc_length, const double distance) { - if (ls.empty()) return segment_t{}; + if (ls.empty()) return Segment2d{}; if (ls.size() == 1lu) return {ls.front(), ls.front()}; auto ls_iterator = ls.begin(); auto prev_arc_length = 0.0; @@ -156,10 +156,10 @@ inline segment_t linestring_to_point_projection( /// @param from_arc_length arc length of the first point of the sub linestring /// @param to_arc_length arc length of the last point of the sub linestring /// @return sub linestring -inline linestring_t sub_linestring( - const linestring_t & ls, const double from_arc_length, const double to_arc_length) +inline LineString2d sub_linestring( + const LineString2d & ls, const double from_arc_length, const double to_arc_length) { - linestring_t sub_ls; + LineString2d sub_ls; if (from_arc_length >= to_arc_length || ls.empty()) throw(std::runtime_error("sub_linestring: bad inputs")); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index e300a347e47a8..7db92c163f567 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -29,26 +29,28 @@ using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; -using point_t = tier4_autoware_utils::Point2d; -using multi_point_t = tier4_autoware_utils::MultiPoint2d; -using polygon_t = tier4_autoware_utils::Polygon2d; -using ring_t = tier4_autoware_utils::LinearRing2d; -using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d; -using segment_t = tier4_autoware_utils::Segment2d; -using linestring_t = tier4_autoware_utils::LineString2d; -using multi_linestring_t = tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::MultiLineString2d; +using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_autoware_utils::Segment2d; struct PointDistance { - point_t point; + Point2d point; double distance; }; struct Projection { - point_t projected_point; + Point2d projected_point; double distance; double arc_length; }; +enum Side { LEFT, RIGHT }; + } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 755817aa22ae7..3fb9060835698 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1003,9 +1003,6 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( updateParam( parameters, DrivableAreaExpansionParameters::AVOID_DYN_OBJECTS_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_dynamic_objects); - updateParam( - parameters, DrivableAreaExpansionParameters::EXPANSION_METHOD_PARAM, - planner_data_->drivable_area_expansion_parameters.expansion_method); updateParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_TYPES_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_types); @@ -1013,17 +1010,14 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::AVOID_LINESTRING_DIST_PARAM, planner_data_->drivable_area_expansion_parameters.avoid_linestring_dist); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_FRONT, - planner_data_->drivable_area_expansion_parameters.ego_extra_front_offset); - updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_REAR, - planner_data_->drivable_area_expansion_parameters.ego_extra_rear_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_FRONT_OVERHANG, + planner_data_->drivable_area_expansion_parameters.extra_front_overhang); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_LEFT, - planner_data_->drivable_area_expansion_parameters.ego_extra_left_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WHEELBASE, + planner_data_->drivable_area_expansion_parameters.extra_wheelbase); updateParam( - parameters, DrivableAreaExpansionParameters::EGO_EXTRA_OFFSET_RIGHT, - planner_data_->drivable_area_expansion_parameters.ego_extra_right_offset); + parameters, DrivableAreaExpansionParameters::EGO_EXTRA_WIDTH, + planner_data_->drivable_area_expansion_parameters.extra_width); updateParam( parameters, DrivableAreaExpansionParameters::DYN_OBJECTS_EXTRA_OFFSET_FRONT, planner_data_->drivable_area_expansion_parameters.dynamic_objects_extra_front_offset); @@ -1046,14 +1040,20 @@ SetParametersResult BehaviorPathPlannerNode::onSetParam( parameters, DrivableAreaExpansionParameters::RESAMPLE_INTERVAL_PARAM, planner_data_->drivable_area_expansion_parameters.resample_interval); updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_ARC_LENGTH_PARAM, - planner_data_->drivable_area_expansion_parameters.extra_arc_length); + parameters, DrivableAreaExpansionParameters::MAX_REUSE_DEVIATION_PARAM, + planner_data_->drivable_area_expansion_parameters.max_reuse_deviation); updateParam( - parameters, DrivableAreaExpansionParameters::COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_uncrossable_lines); + parameters, DrivableAreaExpansionParameters::SMOOTHING_CURVATURE_WINDOW_PARAM, + planner_data_->drivable_area_expansion_parameters.curvature_average_window); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_MAX_BOUND_RATE_PARAM, + planner_data_->drivable_area_expansion_parameters.max_bound_rate); + updateParam( + parameters, DrivableAreaExpansionParameters::SMOOTHING_EXTRA_ARC_LENGTH_PARAM, + planner_data_->drivable_area_expansion_parameters.extra_arc_length); updateParam( - parameters, DrivableAreaExpansionParameters::EXTRA_COMPENSATE_PARAM, - planner_data_->drivable_area_expansion_parameters.compensate_extra_dist); + parameters, DrivableAreaExpansionParameters::PRINT_RUNTIME_PARAM, + planner_data_->drivable_area_expansion_parameters.print_runtime); } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { result.successful = false; result.reason = e.what(); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 60fb2ba2ff2e8..76d1f280651bf 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -14,10 +14,10 @@ #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include @@ -25,304 +25,306 @@ #include #include #include +#include #include +#include + namespace drivable_area_expansion { -std::vector crop_and_resample( - const std::vector & points, - const std::shared_ptr planner_data, - const double resample_interval) +namespace { - auto lon_offset = 0.0; - auto crop_pose = *planner_data->drivable_area_expansion_prev_crop_pose; - // reuse or update the previous crop point - if (planner_data->drivable_area_expansion_prev_crop_pose) { - const auto lon_offset = motion_utils::calcSignedArcLength( - points, points.front().point.pose.position, crop_pose.position); - if (lon_offset < 0.0) { - planner_data->drivable_area_expansion_prev_crop_pose.reset(); - } else { - const auto is_behind_ego = - motion_utils::calcSignedArcLength( - points, crop_pose.position, planner_data->self_odometry->pose.pose.position) > 0.0; - const auto is_too_far = motion_utils::calcLateralOffset(points, crop_pose.position) > 0.1; - if (!is_behind_ego || is_too_far) - planner_data->drivable_area_expansion_prev_crop_pose.reset(); - } - } - if (!planner_data->drivable_area_expansion_prev_crop_pose) { - crop_pose = planner_data->drivable_area_expansion_prev_crop_pose.value_or( - motion_utils::calcInterpolatedPose(points, resample_interval - lon_offset)); - } - // crop - const auto crop_seg_idx = motion_utils::findNearestSegmentIndex(points, crop_pose.position); - const auto cropped_points = motion_utils::cropPoints( - points, crop_pose.position, crop_seg_idx, - planner_data->drivable_area_expansion_parameters.max_path_arc_length, 0.0); - planner_data->drivable_area_expansion_prev_crop_pose = crop_pose; - // resample - PathWithLaneId cropped_path; - if (tier4_autoware_utils::calcDistance2d(crop_pose, cropped_points.front()) > 1e-3) { - PathPointWithLaneId crop_path_point; - crop_path_point.point.pose = crop_pose; - cropped_path.points.push_back(crop_path_point); - } - cropped_path.points.insert( - cropped_path.points.end(), cropped_points.begin(), cropped_points.end()); - const auto resampled_path = - motion_utils::resamplePath(cropped_path, resample_interval, true, true, false); - return resampled_path.points; +Point2d convert_point(const Point & p) +{ + return Point2d{p.x, p.y}; } -void expandDrivableArea( - PathWithLaneId & path, - const std::shared_ptr planner_data, - const lanelet::ConstLanelets & path_lanes) +} // namespace + +void reuse_previous_poses( + const PathWithLaneId & path, std::vector & prev_poses, + std::vector & prev_curvatures, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - const auto & params = planner_data->drivable_area_expansion_parameters; - const auto & dynamic_objects = *planner_data->dynamic_object; - const auto & route_handler = *planner_data->route_handler; - const auto uncrossable_lines = - extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types); - multi_linestring_t uncrossable_lines_in_range; - const auto & p = path.points.front().point.pose.position; - for (const auto & line : uncrossable_lines) - if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length) - uncrossable_lines_in_range.push_back(line); - const auto points = crop_and_resample(path.points, planner_data, params.resample_interval); - const auto path_footprints = createPathFootprints(points, params); - const auto predicted_paths = createObjectFootprints(dynamic_objects, params); - const auto expansion_polygons = - params.expansion_method == "lanelet" - ? createExpansionLaneletPolygons( - path_lanes, route_handler, path_footprints, predicted_paths, params) - : createExpansionPolygons( - path, path_footprints, predicted_paths, uncrossable_lines_in_range, params); - const auto expanded_drivable_area = createExpandedDrivableAreaPolygon(path, expansion_polygons); - updateDrivableAreaBounds(path, expanded_drivable_area); + std::vector cropped_poses; + std::vector cropped_curvatures; + const auto ego_is_behind = prev_poses.size() > 1 && motion_utils::calcLongitudinalOffsetToSegment( + prev_poses, 0, ego_point) < 0.0; + const auto ego_is_far = !prev_poses.empty() && + tier4_autoware_utils::calcDistance2d(ego_point, prev_poses.front()) < 0.0; + if (!ego_is_behind && !ego_is_far && prev_poses.size() > 1) { + const auto first_idx = + motion_utils::findNearestSegmentIndex(prev_poses, path.points.front().point.pose); + const auto deviation = + motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); + if (first_idx && deviation < params.max_reuse_deviation) { + for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { + if ( + motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) > + params.max_reuse_deviation) + break; + cropped_poses.push_back(prev_poses[idx]); + cropped_curvatures.push_back(prev_curvatures[idx]); + } + } + } + if (cropped_poses.empty()) { + const auto resampled_path_points = + motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; + for (const auto & p : resampled_path_points) cropped_poses.push_back(p.point.pose); + } else if (!path.points.empty()) { + const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); + const auto max_path_arc_length = motion_utils::calcArcLength(path.points); + const auto first_arc_length = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, cropped_poses.back().position); + for (auto arc_length = first_arc_length + params.resample_interval; + initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length && + arc_length <= max_path_arc_length; + arc_length += params.resample_interval) + cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); + } + prev_poses = motion_utils::removeOverlapPoints(cropped_poses); + prev_curvatures = cropped_curvatures; } -point_t convert_point(const Point & p) +double calculate_minimum_lane_width( + const double curvature_radius, const DrivableAreaExpansionParameters & params) { - return point_t{p.x, p.y}; + const auto k = curvature_radius; + const auto a = params.vehicle_info.front_overhang_m + params.extra_front_overhang; + const auto w = params.vehicle_info.vehicle_width_m + params.extra_width; + const auto l = params.vehicle_info.wheel_base_m + params.extra_wheelbase; + return (a * a + 2.0 * a * l + 2.0 * k * w + l * l + w * w) / (2.0 * k + w); } -Point convert_point(const point_t & p) +std::vector calculate_minimum_expansions( + const std::vector & path_poses, const std::vector bound, + const std::vector curvatures, const Side side, + const DrivableAreaExpansionParameters & params) { - return Point().set__x(p.x()).set__y(p.y()); + std::vector minimum_expansions(bound.size()); + size_t lb_idx = 0; + for (auto path_idx = 0UL; path_idx < path_poses.size(); ++path_idx) { + const auto & path_pose = path_poses[path_idx]; + if (curvatures[path_idx] == 0.0) continue; + const auto curvature_radius = 1 / curvatures[path_idx]; + const auto min_lane_width = calculate_minimum_lane_width(curvature_radius, params); + const auto side_distance = min_lane_width / 2.0 * (side == LEFT ? 1.0 : -1.0); + const auto offset_point = + tier4_autoware_utils::calcOffsetPose(path_pose, 0.0, side_distance, 0.0).position; + for (auto bound_idx = lb_idx; bound_idx + 1 < bound.size(); ++bound_idx) { + const auto & prev_p = bound[bound_idx]; + const auto & next_p = bound[bound_idx + 1]; + const auto intersection_point = + tier4_autoware_utils::intersect(offset_point, path_pose.position, prev_p, next_p); + if (intersection_point) { + lb_idx = bound_idx; + const auto dist = tier4_autoware_utils::calcDistance2d(*intersection_point, offset_point); + minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); + minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); + // apply the expansion to all bound points within the extra arc length + if (bound_idx + 2 < bound.size()) { + auto up_arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]) + + tier4_autoware_utils::calcDistance2d(bound[bound_idx + 1], bound[bound_idx + 2]); + for (auto up_bound_idx = bound_idx + 2; + bound_idx < bound.size() && up_arc_length <= params.extra_arc_length; + ++up_bound_idx) { + minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); + if (up_bound_idx + 1 < bound.size()) + up_arc_length += + tier4_autoware_utils::calcDistance2d(bound[up_bound_idx], bound[up_bound_idx + 1]); + } + } + if (bound_idx > 0) { + auto down_arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]) + + tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); + for (auto down_bound_idx = bound_idx - 1; + down_bound_idx > 0 && down_arc_length <= params.extra_arc_length; --down_bound_idx) { + minimum_expansions[down_bound_idx] = std::max(minimum_expansions[down_bound_idx], dist); + if (down_bound_idx > 1) + down_arc_length += tier4_autoware_utils::calcDistance2d( + bound[down_bound_idx], bound[down_bound_idx - 1]); + } + } + break; + } + } + } + return minimum_expansions; } -polygon_t createExpandedDrivableAreaPolygon( - const PathWithLaneId & path, const multi_polygon_t & expansion_polygons) +void apply_bound_change_rate_limit( + std::vector & distances, const std::vector & bound, const double max_rate) { - polygon_t original_da_poly; - original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1); - for (const auto & p : path.left_bound) original_da_poly.outer().push_back(convert_point(p)); - for (auto it = path.right_bound.rbegin(); it != path.right_bound.rend(); ++it) - original_da_poly.outer().push_back(convert_point(*it)); - original_da_poly.outer().push_back(original_da_poly.outer().front()); - - multi_polygon_t unions; - auto expanded_da_poly = original_da_poly; - for (const auto & p : expansion_polygons) { - unions.clear(); - boost::geometry::union_(expanded_da_poly, p, unions); - if (unions.size() == 1) // union of overlapping polygons should produce a single polygon - expanded_da_poly = unions[0]; - } - return expanded_da_poly; + if (distances.empty()) return; + const auto apply_max_vel = [&](auto & exp, const auto from, const auto to) { + if (exp[from] > exp[to]) { + const auto arc_length = tier4_autoware_utils::calcDistance2d(bound[from], bound[to]); + const auto smoothed_dist = exp[from] - arc_length * max_rate; + exp[to] = std::max(exp[to], smoothed_dist); + } + }; + for (auto idx = 0LU; idx + 1 < distances.size(); ++idx) apply_max_vel(distances, idx, idx + 1); + for (auto idx = distances.size() - 1; idx > 0; --idx) apply_max_vel(distances, idx, idx - 1); } -void copy_z_over_arc_length( - const std::vector & from, std::vector & to) +std::vector calculate_maximum_distance( + const std::vector & path_poses, const std::vector bound, + const std::vector & uncrossable_lines, + const std::vector & uncrossable_polygons, + const DrivableAreaExpansionParameters & params) { - if (from.empty() || to.empty()) return; - to.front().z = from.front().z; - if (from.size() < 2 || to.size() < 2) return; - to.back().z = from.back().z; - auto i_from = 1lu; - auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]); - auto s_to = 0.0; - auto s_from_prev = 0.0; - for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) { - s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]); - for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) { - s_from_prev = s_from; - s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]); + // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) + std::vector maximum_distances(bound.size(), std::numeric_limits::max()); + LineString2d path_ls; + LineString2d bound_ls; + for (const auto & p : bound) bound_ls.push_back(convert_point(p)); + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); + for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { + const LineString2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + for (const auto & uncrossable_line : uncrossable_lines) { + const auto bound_to_line_dist = boost::geometry::distance(segment_ls, uncrossable_line); + const auto dist_limit = std::max(0.0, bound_to_line_dist - params.avoid_linestring_dist); + maximum_distances[i] = std::min(maximum_distances[i], dist_limit); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); } - if (s_from - s_from_prev != 0.0) { - const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev); - to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio); - } else { - to[i_to].z = to[i_to - 1].z; + for (const auto & uncrossable_poly : uncrossable_polygons) { + const auto bound_to_poly_dist = boost::geometry::distance(segment_ls, uncrossable_poly); + maximum_distances[i] = std::min(maximum_distances[i], bound_to_poly_dist); + maximum_distances[i + 1] = std::min(maximum_distances[i + 1], bound_to_poly_dist); } } + if (params.max_expansion_distance > 0.0) + for (auto & d : maximum_distances) d = std::min(params.max_expansion_distance, d); + return maximum_distances; } -void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area) +void expand_bound( + std::vector & bound, const std::vector & path_poses, + const std::vector & expansions) { - const auto original_left_bound = path.left_bound; - const auto original_right_bound = path.right_bound; - const auto is_left_of_path = [&](const point_t & p) { - return motion_utils::calcLateralOffset(path.points, convert_point(p)) > 0.0; - }; - // prepare delimiting lines: start and end of the original expanded drivable area - const auto start_segment = - segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())}; - const auto end_segment = - segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())}; - point_t start_segment_center; - boost::geometry::centroid(start_segment, start_segment_center); - const auto path_start_segment = - segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)}; - point_t end_segment_center; - boost::geometry::centroid(end_segment, end_segment_center); - const auto path_end_segment = - segment_t{convert_point(path.points.back().point.pose.position), end_segment_center}; - const auto segment_to_line_intersection = - [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional { - const auto line = Eigen::Hyperplane::Through(q1, q2); - const auto segment = Eigen::Hyperplane::Through(p1, p2); - const auto intersection = line.intersection(segment); - std::optional result; - const auto is_on_segment = - (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x() - : intersection.x() <= p1.x() && intersection.x() >= p2.x()) && - (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y() - : intersection.y() <= p1.y() && intersection.y() >= p2.y()); - if (is_on_segment) result = point_t{intersection.x(), intersection.y()}; - return result; - }; - // find intersection between the expanded drivable area and the delimiting lines - const auto & da = expanded_drivable_area.outer(); - struct Intersection - { - point_t intersection_point; - ring_t::const_iterator segment_it; - double distance = std::numeric_limits::max(); - explicit Intersection(ring_t::const_iterator it) : segment_it(it) {} - void update(const point_t & p, const ring_t::const_iterator & it, const double dist) - { - intersection_point = p; - segment_it = it; - distance = dist; - } - }; - Intersection start_left(da.end()); - Intersection end_left(da.end()); - Intersection start_right(da.end()); - Intersection end_right(da.end()); - for (auto it = da.begin(); it != da.end(); ++it) { - if (boost::geometry::distance(*it, start_segment.first) < 1e-3) - start_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, start_segment.second) < 1e-3) - start_right.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.first) < 1e-3) - end_left.update(*it, it, 0.0); - else if (boost::geometry::distance(*it, end_segment.second) < 1e-3) - end_right.update(*it, it, 0.0); - const auto inter_start = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second) - : segment_to_line_intersection( - *it, *std::next(it), start_segment.first, start_segment.second); - if (inter_start) { - const auto dist = boost::geometry::distance(*inter_start, path_start_segment); - const auto is_left = is_left_of_path(*inter_start); - if (is_left && dist < start_left.distance) - start_left.update(*inter_start, it, dist); - else if (!is_left && dist < start_right.distance) - start_right.update(*inter_start, it, dist); + LineString2d path_ls; + for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); + for (auto idx = 0LU; idx < bound.size(); ++idx) { + const auto bound_p = convert_point(bound[idx]); + const auto projection = point_to_linestring_projection(bound_p, path_ls); + const auto expansion_ratio = + (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); + if (expansion_ratio > 1.0) { + const auto & path_p = projection.projected_point; + const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); + bound[idx].x = expanded_p.x(); + bound[idx].y = expanded_p.y(); } - const auto inter_end = - std::next(it) == da.end() - ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second) - : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second); - if (inter_end) { - const auto dist = boost::geometry::distance(*inter_end, path_end_segment); - const auto is_left = is_left_of_path(*inter_end); - if (is_left && dist < end_left.distance) - end_left.update(*inter_end, it, dist); - else if (!is_left && dist < end_right.distance) - end_right.update(*inter_end, it, dist); - } - } - if (start_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.first) < - boost::geometry::distance(b, start_segment.first); - }); - start_left.update(*closest_it, closest_it, 0.0); - } - if (start_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, start_segment.second) < - boost::geometry::distance(b, start_segment.second); - }); - start_right.update(*closest_it, closest_it, 0.0); - } - if (end_left.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.first) < - boost::geometry::distance(b, end_segment.first); - }); - end_left.update(*closest_it, closest_it, 0.0); - } - if (end_right.segment_it == da.end()) { - const auto closest_it = - std::min_element(da.begin(), da.end(), [&](const auto & a, const auto & b) { - return boost::geometry::distance(a, end_segment.second) < - boost::geometry::distance(b, end_segment.second); - }); - end_right.update(*closest_it, closest_it, 0.0); } - // extract the expanded left and right bound from the expanded drivable area - path.left_bound.clear(); - path.right_bound.clear(); - path.left_bound.push_back(convert_point(start_left.intersection_point)); - path.right_bound.push_back(convert_point(start_right.intersection_point)); - if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it)) - path.right_bound.push_back(convert_point(*start_right.segment_it)); - if (start_left.segment_it < end_left.segment_it) { - for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::next(start_left.segment_it); it < da.end(); ++it) - path.left_bound.push_back(convert_point(*it)); - for (auto it = da.begin(); it <= end_left.segment_it; ++it) - path.left_bound.push_back(convert_point(*it)); + // remove any self intersection by skipping the points inside of the loop + std::vector no_loop_bound = {bound.front()}; + for (auto idx = 1LU; idx < bound.size(); ++idx) { + bool is_intersecting = false; + for (auto succ_idx = idx + 1; succ_idx < bound.size(); ++succ_idx) { + const auto intersection = tier4_autoware_utils::intersect( + bound[idx - 1], bound[idx], bound[succ_idx - 1], bound[succ_idx]); + if ( + intersection && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx - 1]) < 1e-3 && + tier4_autoware_utils::calcDistance2d(*intersection, bound[idx]) < 1e-3) { + idx = succ_idx; + is_intersecting = true; + } + } + if (!is_intersecting) no_loop_bound.push_back(bound[idx]); } - if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it)) - path.left_bound.push_back(convert_point(end_left.intersection_point)); - if (start_right.segment_it < end_right.segment_it) { - for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it) - path.right_bound.push_back(convert_point(*it)); - for (auto it = std::prev(da.end()); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); - } else { - for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it) - path.right_bound.push_back(convert_point(*it)); + bound = no_loop_bound; +} + +std::vector calculate_smoothed_curvatures( + const std::vector & poses, const size_t smoothing_window_size) +{ + const auto curvatures = motion_utils::calcCurvature(poses); + std::vector smoothed_curvatures(curvatures.size()); + for (auto i = 0UL; i < curvatures.size(); ++i) { + auto sum = 0.0; + const auto from_idx = (i >= smoothing_window_size ? i - smoothing_window_size : 0); + const auto to_idx = std::min(i + smoothing_window_size, curvatures.size() - 1); + for (auto j = from_idx; j <= to_idx; ++j) sum += std::abs(curvatures[j]); + smoothed_curvatures[i] = sum / static_cast(to_idx - from_idx + 1); } - if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it))) - path.right_bound.push_back(convert_point(end_right.intersection_point)); - // remove possible duplicated points - const auto point_cmp = [](const auto & p1, const auto & p2) { - return p1.x == p2.x && p1.y == p2.y; - }; - path.left_bound.erase( - std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp), path.left_bound.end()); - path.right_bound.erase( - std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp), - path.right_bound.end()); - copy_z_over_arc_length(original_left_bound, path.left_bound); - copy_z_over_arc_length(original_right_bound, path.right_bound); + return smoothed_curvatures; +} + +void expand_drivable_area( + PathWithLaneId & path, + const std::shared_ptr planner_data) +{ + // skip if no bounds or not enough points to calculate path curvature + if (path.points.size() < 3 || path.left_bound.empty() || path.right_bound.empty()) return; + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("overall"); + stop_watch.tic("preprocessing"); + // crop first/last non deviating path_poses + const auto & params = planner_data->drivable_area_expansion_parameters; + const auto & route_handler = *planner_data->route_handler; + const auto uncrossable_lines = extract_uncrossable_lines( + *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); + const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); + const auto preprocessing_ms = stop_watch.toc("preprocessing"); + + stop_watch.tic("crop"); + std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; + std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; + reuse_previous_poses( + path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); + const auto crop_ms = stop_watch.toc("crop"); + + stop_watch.tic("curvatures_expansion"); + // Only add curvatures for the new points. Curvatures of reused path points are not updated. + const auto new_curvatures = + calculate_smoothed_curvatures(path_poses, params.curvature_average_window); + const auto first_new_point_idx = curvatures.size(); + curvatures.insert( + curvatures.end(), new_curvatures.begin() + first_new_point_idx, new_curvatures.end()); + auto left_expansions = + calculate_minimum_expansions(path_poses, path.left_bound, curvatures, LEFT, params); + auto right_expansions = + calculate_minimum_expansions(path_poses, path.right_bound, curvatures, RIGHT, params); + const auto curvature_expansion_ms = stop_watch.toc("curvatures_expansion"); + + stop_watch.tic("max_dist"); + const auto max_left_expansions = calculate_maximum_distance( + path_poses, path.left_bound, uncrossable_lines, uncrossable_polygons, params); + const auto max_right_expansions = calculate_maximum_distance( + path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons, params); + for (auto i = 0LU; i < left_expansions.size(); ++i) + left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); + for (auto i = 0LU; i < right_expansions.size(); ++i) + right_expansions[i] = std::min(right_expansions[i], max_right_expansions[i]); + const auto max_dist_ms = stop_watch.toc("max_dist"); + + stop_watch.tic("smooth"); + apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); + apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); + const auto smooth_ms = stop_watch.toc("smooth"); + // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) + stop_watch.tic("expand"); + expand_bound(path.left_bound, path_poses, left_expansions); + expand_bound(path.right_bound, path_poses, right_expansions); + const auto expand_ms = stop_watch.toc("expand"); + + const auto total_ms = stop_watch.toc("overall"); + if (params.print_runtime) + std::printf( + "Total runtime(ms): %2.2f\n\tPreprocessing: %2.2f\n\tCrop: %2.2f\n\tCurvature expansion: " + "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", + total_ms, preprocessing_ms, crop_ms, curvature_expansion_ms, max_dist_ms, smooth_ms, + expand_ms); + + planner_data->drivable_area_expansion_prev_path_poses = path_poses; + planner_data->drivable_area_expansion_prev_curvatures = curvatures; } } // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp deleted file mode 100644 index 828fdc2f17a51..0000000000000 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp +++ /dev/null @@ -1,237 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" - -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" - -namespace drivable_area_expansion -{ - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_linestring_t & limit_lines) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & line : limit_lines) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, limit_lines, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - for (const auto & p : line) - if (boost::geometry::within(p, expansion_polygon)) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -double calculateDistanceLimit( - const linestring_t & base_ls, const polygon_t & expansion_polygon, - const multi_polygon_t & limit_polygons) -{ - auto dist_limit = std::numeric_limits::max(); - for (const auto & polygon : limit_polygons) { - multi_point_t intersections; - boost::geometry::intersection(expansion_polygon, polygon, intersections); - for (const auto & p : intersections) - dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls)); - } - return dist_limit; -} - -polygon_t createExpansionPolygon( - const linestring_t & base_ls, const double dist, const bool is_left_side) -{ - namespace strategy = boost::geometry::strategy::buffer; - multi_polygon_t polygons; - // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer - constexpr auto zero = 0.1; - const auto left_dist = is_left_side ? dist : zero; - const auto right_dist = !is_left_side ? dist : zero; - const auto distance_strategy = strategy::distance_asymmetric(left_dist, right_dist); - boost::geometry::buffer( - base_ls, polygons, distance_strategy, strategy::side_straight(), strategy::join_miter(), - strategy::end_flat(), strategy::point_square()); - return polygons.front(); -} - -std::array calculate_arc_length_range_and_distance( - const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound, - const bool is_left, const double path_length) -{ - multi_point_t intersections; - double expansion_dist = 0.0; - double from_arc_length = std::numeric_limits::max(); - double to_arc_length = std::numeric_limits::min(); - boost::geometry::intersection(footprint, bound, intersections); - if (!intersections.empty()) { - for (const auto & intersection : intersections) { - const auto projection = point_to_linestring_projection(intersection, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length) continue; - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - for (const auto & p : footprint.outer()) { - const auto projection = point_to_linestring_projection(p, path_ls); - if (projection.arc_length <= 0.0 || projection.arc_length >= path_length - 1e-3) continue; - if (is_left == (projection.distance > 0) && std::abs(projection.distance) > expansion_dist) { - expansion_dist = std::abs(projection.distance); - from_arc_length = std::min(from_arc_length, projection.arc_length); - to_arc_length = std::max(to_arc_length, projection.arc_length); - } - } - } - return std::array({from_arc_length, to_arc_length, expansion_dist}); -} - -polygon_t create_compensation_polygon( - const linestring_t & base_ls, const double compensation_dist, const bool is_left, - const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths) -{ - polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left); - double dist_limit = std::min( - compensation_dist, calculateDistanceLimit(base_ls, compensation_polygon, uncrossable_lines)); - if (!predicted_paths.empty()) - dist_limit = - std::min(dist_limit, calculateDistanceLimit(base_ls, compensation_polygon, predicted_paths)); - if (dist_limit < compensation_dist) - compensation_polygon = createExpansionPolygon(base_ls, dist_limit, !is_left); - return compensation_polygon; -} - -multi_polygon_t createExpansionPolygons( - const PathWithLaneId & path, const multi_polygon_t & path_footprints, - const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines, - const DrivableAreaExpansionParameters & params) -{ - linestring_t path_ls; - linestring_t left_ls; - linestring_t right_ls; - for (const auto & p : path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y); - for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y); - // extend the path linestring to the beginning and end of the drivable area - if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) { - const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]); - const auto right_proj_begin = - point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]); - const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls); - const auto right_ls_proj_begin = - point_to_linestring_projection(right_proj_begin.point, path_ls); - if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length) - path_ls.insert(path_ls.begin(), left_proj_begin.point); - else - path_ls.insert(path_ls.begin(), right_proj_begin.point); - const auto left_proj_end = - point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto right_proj_end = - point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back()); - const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls); - const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls); - if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length) - path_ls.push_back(left_proj_end.point); - else - path_ls.push_back(right_proj_end.point); - } - const auto path_length = static_cast(boost::geometry::length(path_ls)); - - multi_polygon_t expansion_polygons; - for (const auto & footprint : path_footprints) { - bool is_left = true; - for (const auto & bound : {left_ls, right_ls}) { - auto [from_arc_length, to_arc_length, footprint_dist] = - calculate_arc_length_range_and_distance(path_ls, footprint, bound, is_left, path_length); - if (footprint_dist > 0.0) { - from_arc_length -= params.extra_arc_length; - to_arc_length += params.extra_arc_length; - from_arc_length = std::max(0.0, from_arc_length); - to_arc_length = std::min(path_length, to_arc_length); - const auto base_ls = sub_linestring(path_ls, from_arc_length, to_arc_length); - const auto expansion_dist = params.max_expansion_distance != 0.0 - ? std::min(params.max_expansion_distance, footprint_dist) - : footprint_dist; - auto expansion_polygon = createExpansionPolygon(base_ls, expansion_dist, is_left); - auto limited_dist = expansion_dist; - const auto uncrossable_dist_limit = std::max( - 0.0, calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines) - - params.avoid_linestring_dist); - if (uncrossable_dist_limit < limited_dist) { - limited_dist = uncrossable_dist_limit; - if (params.compensate_uncrossable_lines) { - const auto compensation_dist = - footprint_dist - limited_dist + params.compensate_extra_dist; - expansion_polygons.push_back(create_compensation_polygon( - base_ls, compensation_dist, is_left, uncrossable_lines, predicted_paths)); - } - } - limited_dist = std::min( - limited_dist, calculateDistanceLimit(base_ls, expansion_polygon, predicted_paths)); - if (limited_dist < expansion_dist) - expansion_polygon = createExpansionPolygon(base_ls, limited_dist, is_left); - expansion_polygons.push_back(expansion_polygon); - } - is_left = false; - } - } - return expansion_polygons; -} - -multi_polygon_t createExpansionLaneletPolygons( - const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler, - const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths, - const DrivableAreaExpansionParameters & params) -{ - multi_polygon_t expansion_polygons; - lanelet::ConstLanelets candidates; - const auto already_added = [&](const auto & ll) { - return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) != candidates.end(); - }; - const auto add_if_valid = [&](const auto & ll, const auto is_left) { - const auto bound_to_check = is_left ? ll.rightBound() : ll.leftBound(); - if (std::find_if(path_lanes.begin(), path_lanes.end(), [&](const auto & l) { - return ll.id() == l.id(); - }) == path_lanes.end()) - if (!already_added(ll) && !hasTypes(bound_to_check, params.avoid_linestring_types)) - candidates.push_back(ll); - }; - for (const auto & current_ll : path_lanes) { - for (const auto & left_ll : - route_handler.getLaneletsFromPoint(current_ll.leftBound3d().front())) - add_if_valid(left_ll, true); - for (const auto & left_ll : route_handler.getLaneletsFromPoint(current_ll.leftBound3d().back())) - add_if_valid(left_ll, true); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().front())) - add_if_valid(right_ll, false); - for (const auto & right_ll : - route_handler.getLaneletsFromPoint(current_ll.rightBound3d().back())) - add_if_valid(right_ll, false); - } - for (const auto & candidate : candidates) { - polygon_t candidate_poly; - for (const auto & p : candidate.polygon2d()) candidate_poly.outer().emplace_back(p.x(), p.y()); - boost::geometry::correct(candidate_poly); - if ( - !boost::geometry::overlaps(candidate_poly, predicted_paths) && - boost::geometry::overlaps(path_footprints, candidate_poly)) - expansion_polygons.push_back(candidate_poly); - } - return expansion_polygons; -} - -} // namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 4a45dce6a0bcc..2e6d4929fdf02 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -28,63 +28,39 @@ namespace drivable_area_expansion { -polygon_t translatePolygon(const polygon_t & polygon, const double x, const double y) +Polygon2d translate_polygon(const Polygon2d & polygon, const double x, const double y) { - polygon_t translated_polygon; + Polygon2d translated_polygon; const boost::geometry::strategy::transform::translate_transformer translation(x, y); boost::geometry::transform(polygon, translated_polygon, translation); return translated_polygon; } -polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t base_footprint) +Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2d base_footprint) { const auto angle = tf2::getYaw(pose.orientation); - return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); + return translate_polygon( + tier4_autoware_utils::rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y); } -multi_polygon_t createObjectFootprints( +MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { - multi_polygon_t footprints; + MultiPolygon2d footprints; if (params.avoid_dynamic_objects) { for (const auto & object : objects.objects) { const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset; const auto rear = -object.shape.dimensions.x / 2 - params.dynamic_objects_extra_rear_offset; const auto left = object.shape.dimensions.y / 2 + params.dynamic_objects_extra_left_offset; const auto right = -object.shape.dimensions.y / 2 - params.dynamic_objects_extra_right_offset; - polygon_t base_footprint; + Polygon2d base_footprint; base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; + Point2d{front, left}, Point2d{front, right}, Point2d{rear, right}, Point2d{rear, left}, + Point2d{front, left}}; for (const auto & path : object.kinematics.predicted_paths) for (const auto & pose : path.path) - footprints.push_back(createFootprint(pose, base_footprint)); - } - } - return footprints; -} - -multi_polygon_t createPathFootprints( - const std::vector & points, const DrivableAreaExpansionParameters & params) -{ - const auto left = params.ego_left_offset + params.ego_extra_left_offset; - const auto right = params.ego_right_offset - params.ego_extra_right_offset; - const auto rear = params.ego_rear_offset - params.ego_extra_rear_offset; - const auto front = params.ego_front_offset + params.ego_extra_front_offset; - polygon_t base_footprint; - base_footprint.outer() = { - point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left}, - point_t{front, left}}; - multi_polygon_t footprints; - // skip the last footprint as its orientation is usually wrong - footprints.reserve(points.size() - 1); - double arc_length = 0.0; - for (auto it = points.begin(); std::next(it) != points.end(); ++it) { - footprints.push_back(createFootprint(it->point.pose, base_footprint)); - if (params.max_path_arc_length > 0.0) { - arc_length += tier4_autoware_utils::calcDistance2d(it->point.pose, std::next(it)->point.pose); - if (arc_length > params.max_path_arc_length) break; + footprints.push_back(create_footprint(pose, base_footprint)); } } return footprints; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index ded67c251f7ae..deeb787cf39f6 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -15,33 +15,34 @@ #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" -#include "lanelet2_core/primitives/LineString.h" - #include #include +#include #include namespace drivable_area_expansion { -multi_linestring_t extractUncrossableLines( - const lanelet::LaneletMap & lanelet_map, const std::vector & uncrossable_types) +MultiLineString2d extract_uncrossable_lines( + const lanelet::LaneletMap & lanelet_map, const Point & ego_point, + const DrivableAreaExpansionParameters & params) { - multi_linestring_t lines; - linestring_t line; + MultiLineString2d uncrossable_lines_in_range; + LineString2d line; + const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { - if (hasTypes(ls, uncrossable_types)) { + if (has_types(ls, params.avoid_linestring_types)) { line.clear(); - for (const auto & p : ls) line.push_back(point_t{p.x(), p.y()}); - lines.push_back(line); + for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); + if (boost::geometry::distance(line, ego_p) < params.max_path_arc_length) + uncrossable_lines_in_range.push_back(line); } } - return lines; + return uncrossable_lines_in_range; } -bool hasTypes(const lanelet::ConstLineString3d & ls, const std::vector & types) +bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types) { constexpr auto no_type = ""; const auto type = ls.attributeOr(lanelet::AttributeName::Type, no_type); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index bf6fffda41863..cb0486b6cd01b 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1539,7 +1539,7 @@ void generateDrivableArea( } const auto & expansion_params = planner_data->drivable_area_expansion_parameters; if (expansion_params.enabled) { - drivable_area_expansion::expandDrivableArea(path, planner_data, transformed_lanes); + drivable_area_expansion::expand_drivable_area(path, planner_data); } // make bound longitudinally monotonic diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 163221951d726..99cd5cac2b9d9 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/expansion.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" @@ -22,9 +21,9 @@ #include #include -using drivable_area_expansion::linestring_t; -using drivable_area_expansion::point_t; -using drivable_area_expansion::segment_t; +using drivable_area_expansion::LineString2d; +using drivable_area_expansion::Point2d; +using drivable_area_expansion::Segment2d; constexpr auto eps = 1e-9; TEST(DrivableAreaExpansionProjection, PointToSegment) @@ -32,56 +31,56 @@ TEST(DrivableAreaExpansionProjection, PointToSegment) using drivable_area_expansion::point_to_segment_projection; { - point_t query(1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 1.0, eps); EXPECT_NEAR(projection.point.x(), 1.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(-1.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(-1.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(11.0, 1.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(11.0, 1.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, std::sqrt(2), eps); EXPECT_NEAR(projection.point.x(), 10.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(10.0, 0.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(10.0, 0.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -5.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 0.0, eps); } { - point_t query(5.0, -5.0); - segment_t segment(point_t(0.0, 0.0), point_t(0.0, -10.0)); + Point2d query(5.0, -5.0); + Segment2d segment(Point2d(0.0, 0.0), Point2d(0.0, -10.0)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 5.0, eps); EXPECT_NEAR(projection.point.x(), 0.0, eps); EXPECT_NEAR(projection.point.y(), -5.0, eps); } { - point_t query(5.0, 5.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(5.0, 5.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, 0.0, eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); EXPECT_NEAR(projection.point.y(), 5.0, eps); } { - point_t query(0.0, 0.0); - segment_t segment(point_t(2.5, 7.5), point_t(7.5, 2.5)); + Point2d query(0.0, 0.0); + Segment2d segment(Point2d(2.5, 7.5), Point2d(7.5, 2.5)); const auto projection = point_to_segment_projection(query, segment.first, segment.second); EXPECT_NEAR(projection.distance, -std::sqrt(50), eps); EXPECT_NEAR(projection.point.x(), 5.0, eps); @@ -93,11 +92,11 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) { using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; { - point_t query(0.0, 0.0); + Point2d query(0.0, 0.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 0.0, eps); EXPECT_NEAR(projection.distance, 0.0, eps); @@ -105,7 +104,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(2.0, 1.0); + Point2d query(2.0, 1.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 2.0, eps); EXPECT_NEAR(projection.distance, 1.0, eps); @@ -113,7 +112,7 @@ TEST(DrivableAreaExpansionProjection, PointToLinestring) EXPECT_NEAR(projection.projected_point.y(), 0.0, eps); } { - point_t query(0.0, 5.0); + Point2d query(0.0, 5.0); const auto projection = point_to_linestring_projection(query, ls); EXPECT_NEAR(projection.arc_length, 30.0 + std::sqrt(2.5 * 2.5 * 2), eps); EXPECT_NEAR(projection.distance, -std::sqrt(2.5 * 2.5 * 2), eps); @@ -126,9 +125,9 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) { using drivable_area_expansion::linestring_to_point_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto arc_length = 0.0; arc_length <= 10.0; arc_length += 1.0) { const auto projection = linestring_to_point_projection(ls, arc_length, 0.0); EXPECT_NEAR(projection.first.x(), arc_length, eps); @@ -152,58 +151,18 @@ TEST(DrivableAreaExpansionProjection, LinestringToPoint) } } -TEST(DrivableAreaExpansionProjection, SubLinestring) -{ - using drivable_area_expansion::sub_linestring; - - const linestring_t ls = { - point_t{0.0, 0.0}, point_t{1.0, 0.0}, point_t{2.0, 0.0}, point_t{3.0, 0.0}, - point_t{4.0, 0.0}, point_t{5.0, 0.0}, point_t{6.0, 0.0}, - }; - { - // arc lengths equal to the original range: same linestring is returned - const auto sub = sub_linestring(ls, 0.0, 6.0); - ASSERT_EQ(ls.size(), sub.size()); - for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - } - { - // arc lengths equal to existing point: sub-linestring with same points - const auto sub = sub_linestring(ls, 1.0, 5.0); - ASSERT_EQ(ls.size() - 2lu, sub.size()); - for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i])); - } - { - // arc lengths inside the original: sub-linestring with some interpolated points - const auto sub = sub_linestring(ls, 1.5, 2.5); - ASSERT_EQ(sub.size(), 3lu); - EXPECT_NEAR(sub[0].x(), 1.5, eps); - EXPECT_NEAR(sub[1].x(), 2.0, eps); - EXPECT_NEAR(sub[2].x(), 2.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } - { - // arc length outside of the original range: first & last point are replaced by interpolations - const auto sub = sub_linestring(ls, -0.5, 8.5); - ASSERT_EQ(sub.size(), ls.size()); - EXPECT_NEAR(sub.front().x(), -0.5, eps); - for (auto i = 1lu; i + 1 < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i])); - EXPECT_NEAR(sub.back().x(), 8.5, eps); - for (const auto & p : sub) EXPECT_NEAR(p.y(), 0.0, eps); - } -} - TEST(DrivableAreaExpansionProjection, InverseProjection) { using drivable_area_expansion::linestring_to_point_projection; using drivable_area_expansion::point_to_linestring_projection; - linestring_t ls = { - point_t(0.0, 0.0), point_t(10.0, 0.0), point_t(10.0, 10.0), point_t(0.0, 10.0), - point_t(5.0, 5.0)}; + LineString2d ls = { + Point2d(0.0, 0.0), Point2d(10.0, 0.0), Point2d(10.0, 10.0), Point2d(0.0, 10.0), + Point2d(5.0, 5.0)}; for (auto x = 0.0; x < 10.0; x += 0.1) { for (auto y = 0.0; x < 10.0; x += 0.1) { - point_t p(x, y); + Point2d p(x, y); const auto projection = point_to_linestring_projection(p, ls); const auto inverse = linestring_to_point_projection(ls, projection.arc_length, projection.distance); @@ -213,7 +172,7 @@ TEST(DrivableAreaExpansionProjection, InverseProjection) } } -TEST(DrivableAreaExpansionProjection, expandDrivableArea) +TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; @@ -256,121 +215,59 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea) params.avoid_dynamic_objects = false; params.avoid_linestring_dist = 0.0; params.avoid_linestring_types = {}; - params.compensate_extra_dist = false; params.max_expansion_distance = 0.0; // means no limit params.max_path_arc_length = 0.0; // means no limit params.resample_interval = 1.0; - params.extra_arc_length = 1.0; - params.expansion_method = "polygon"; // 2m x 4m ego footprint - params.ego_front_offset = 1.0; - params.ego_rear_offset = -1.0; - params.ego_left_offset = 2.0; - params.ego_right_offset = -2.0; + params.vehicle_info.front_overhang_m = 0.0; + params.vehicle_info.wheel_base_m = 2.0; + params.vehicle_info.vehicle_width_m = 2.0; } behavior_path_planner::PlannerData planner_data; planner_data.drivable_area_expansion_parameters = params; planner_data.dynamic_object = std::make_shared(dynamic_objects); + planner_data.self_odometry = std::make_shared(); planner_data.route_handler = std::make_shared(route_handler); - // we expect the drivable area to be expanded by 1m on each side - drivable_area_expansion::expandDrivableArea( - path, std::make_shared(planner_data), path_lanes); + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); // unchanged path points ASSERT_EQ(path.points.size(), 3ul); for (auto i = 0.0; i < path.points.size(); ++i) { EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps); EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps); } - + // straight path: no expansion // expanded left bound - ASSERT_EQ(path.left_bound.size(), 4ul); + ASSERT_EQ(path.left_bound.size(), 3ul); EXPECT_NEAR(path.left_bound[0].x, 0.0, eps); EXPECT_NEAR(path.left_bound[0].y, 1.0, eps); - EXPECT_NEAR(path.left_bound[1].x, 0.0, eps); - EXPECT_NEAR(path.left_bound[1].y, 2.0, eps); + EXPECT_NEAR(path.left_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.left_bound[1].y, 1.0, eps); EXPECT_NEAR(path.left_bound[2].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[2].y, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].x, 2.0, eps); - EXPECT_NEAR(path.left_bound[3].y, 1.0, eps); + EXPECT_NEAR(path.left_bound[2].y, 1.0, eps); // expanded right bound ASSERT_EQ(path.right_bound.size(), 3ul); EXPECT_NEAR(path.right_bound[0].x, 0.0, eps); - EXPECT_NEAR(path.right_bound[0].y, -2.0, eps); - EXPECT_NEAR(path.right_bound[1].x, 2.0, eps); - EXPECT_NEAR(path.right_bound[1].y, -2.0, eps); + EXPECT_NEAR(path.right_bound[0].y, -1.0, eps); + EXPECT_NEAR(path.right_bound[1].x, 1.0, eps); + EXPECT_NEAR(path.right_bound[1].y, -1.0, eps); EXPECT_NEAR(path.right_bound[2].x, 2.0, eps); EXPECT_NEAR(path.right_bound[2].y, -1.0, eps); -} - -TEST(DrivableAreaExpansion, calculateDistanceLimit) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::multi_linestring_t; - using drivable_area_expansion::polygon_t; - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = {}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, std::numeric_limits::max(), 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const linestring_t uncrossable_line = {{0.0, 2.0}, {10.0, 2.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_line}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const multi_linestring_t uncrossable_lines = { - {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, uncrossable_lines); - EXPECT_NEAR(limit_distance, 1.0, 1e-9); - } -} - -TEST(DrivableAreaExpansion, calculateDistanceLimitEdgeCases) -{ - using drivable_area_expansion::calculateDistanceLimit; - using drivable_area_expansion::linestring_t; - using drivable_area_expansion::polygon_t; + // add some curvature + path.points[1].point.pose.position.y = 0.5; - const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}}; - const polygon_t expansion_polygon = { - {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {0.0, -4.0}}, {}}; - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } - { // intersection points further than the line point inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 5.0}, {5.0, 2.0}, {6.0, 4.5}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 2.0}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 2.0, 1e-9); - } - { // line completely inside the expansion polygon - const linestring_t uncrossable_lines = {{4.0, 3.5}, {6.0, 3.0}}; - const auto limit_distance = - calculateDistanceLimit(base_ls, expansion_polygon, {uncrossable_lines}); - EXPECT_NEAR(limit_distance, 3.0, 1e-9); - } + drivable_area_expansion::expand_drivable_area( + path, std::make_shared(planner_data)); + // expanded left bound + ASSERT_EQ(path.left_bound.size(), 3ul); + EXPECT_GT(path.left_bound[0].y, 1.0); + EXPECT_GT(path.left_bound[1].y, 1.0); + EXPECT_GT(path.left_bound[2].y, 1.0); + // expanded right bound + ASSERT_EQ(path.right_bound.size(), 3ul); + EXPECT_LT(path.right_bound[0].y, -1.0); + EXPECT_LT(path.right_bound[1].y, -1.0); + EXPECT_LT(path.right_bound[2].y, -1.0); } From 78d4d8247fda3192f8b7032286b56c81fce74c4e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 16 Oct 2023 17:59:36 +0900 Subject: [PATCH 099/206] fix(lane_change): fix filtering objects (#5317) Signed-off-by: kosuke55 --- .../scene_module/lane_change/normal.hpp | 3 ++- .../src/scene_module/lane_change/normal.cpp | 22 +++++++++---------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index ccadcd7144a9b..8400cf8c40afd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -151,7 +151,8 @@ class NormalLaneChange : public LaneChangeBase CollisionCheckDebugMap & debug_data) const; LaneChangeTargetObjectIndices filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 12b6d53a83220..1012c228e8607 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -732,7 +732,9 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; + auto objects = *planner_data_->dynamic_object; + utils::path_safety_checker::filterObjectsByClass( + objects, lane_change_parameters_->object_types_to_check); // get backward lanes const auto backward_length = lane_change_parameters_->backward_lane_length; @@ -740,7 +742,8 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( route_handler, target_lanes, current_pose, backward_length); // filter objects to get target object index - const auto target_obj_index = filterObject(current_lanes, target_lanes, target_backward_lanes); + const auto target_obj_index = + filterObject(objects, current_lanes, target_lanes, target_backward_lanes); LaneChangeTargetObjects target_objects; target_objects.current_lane.reserve(target_obj_index.current_lane.size()); @@ -772,13 +775,13 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects( } LaneChangeTargetObjectIndices NormalLaneChange::filterObject( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, + const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const { const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; - const auto & objects = *planner_data_->dynamic_object; // Guard if (objects.objects.empty()) { @@ -818,15 +821,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( target_backward_polygons.push_back(lane_polygon); } - auto filtered_objects = objects; - - utils::path_safety_checker::filterObjectsByClass( - filtered_objects, lane_change_parameters_->object_types_to_check); - LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < filtered_objects.objects.size(); ++i) { - const auto & object = filtered_objects.objects.at(i); - const auto & obj_velocity_norm = std::hypot( + for (size_t i = 0; i < objects.objects.size(); ++i) { + const auto & object = objects.objects.at(i); + const auto obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto extended_object = From dad4722a309cca23304bf449cd7a266db942efb9 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 16 Oct 2023 18:11:07 +0900 Subject: [PATCH 100/206] fix(lane_change): do not use reference values of polygon outer (#5318) * fix(lane_change): do not use refelence values of polygon outer Signed-off-by: Fumiya Watanabe * fix(lane_change): fix in lane change Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../src/scene_module/lane_change/normal.cpp | 3 ++- .../src/utils/path_safety_checker/safety_check.cpp | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 1012c228e8607..769b0d681d2ac 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -835,7 +835,8 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // calc distance from the current ego position double max_dist_ego_to_obj = std::numeric_limits::lowest(); double min_dist_ego_to_obj = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const double dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p); max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj); diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index e4698e98d46eb..e36115c62e148 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -51,7 +51,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0); // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (tier4_autoware_utils::calcLongitudinalDeviation(ego_offset_pose, obj_point) > 0.0) { return true; @@ -70,7 +71,8 @@ bool isTargetObjectFront( tier4_autoware_utils::calcOffsetPose(ego_pose, base_to_front, 0.0, 0.0).position; // check all edges in the polygon - for (const auto & obj_edge : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & obj_edge : obj_polygon_outer) { const auto obj_point = tier4_autoware_utils::createPoint(obj_edge.x(), obj_edge.y(), 0.0); if (motion_utils::isTargetPointFront(path.points, ego_point, obj_point)) { return true; @@ -134,7 +136,8 @@ Polygon2d createExtendedPolygon( double min_x = std::numeric_limits::max(); double max_y = std::numeric_limits::lowest(); double min_y = std::numeric_limits::max(); - for (const auto & polygon_p : obj_polygon.outer()) { + const auto obj_polygon_outer = obj_polygon.outer(); + for (const auto & polygon_p : obj_polygon_outer) { const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); const auto transformed_p = tier4_autoware_utils::inverseTransformPoint(obj_p, obj_pose); From fb35f6216ae57ec72ff30f173198d2d70ba99f31 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 17 Oct 2023 00:13:17 +0900 Subject: [PATCH 101/206] feat(tier4_perception_launch): add radar far object integration in tracking stage (#5269) * update tracking/perception launch Signed-off-by: yoshiri * switch tracker launcher mode with argument Signed-off-by: yoshiri * update prediction to switch by radar_long_range_integration paramter Signed-off-by: yoshiri * make radar far object integration switchable between detection/tracking Signed-off-by: yoshiri * fix camera lidar radar fusion flow when 'tracking' is used. Signed-off-by: yoshiri * fix spelling and appearance Signed-off-by: yoshiri * reconstruct topic flow when use tracking to merge far object detection and near object detection Signed-off-by: yoshiri * fix input topic miss in tracking.launch Signed-off-by: yoshiri * fix comment in camera_lidar_radar fusion Signed-off-by: yoshiri * refactor: rename and remove paramters in prediction.launch Signed-off-by: yoshiri * refactor: rename merger control variable from string to bool Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- ...ar_radar_fusion_based_detection.launch.xml | 31 ++++++----- .../prediction/prediction.launch.xml | 3 +- .../tracking/tracking.launch.xml | 53 ++++++++++++++----- .../launch/perception.launch.xml | 15 ++++++ 4 files changed, 75 insertions(+), 27 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index fbe5f21c041b6..7d0988635fb10 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -51,6 +51,10 @@ + + + + - - - - - - - - - + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml index 711e6374f5786..fa343f49840ad 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -1,12 +1,13 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 77de1e5995ee0..07b53fb671732 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -3,29 +3,54 @@ + + + + + + + + - + + + - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index bbb7ebb2d8d25..63ea0cc7f0f3d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -96,6 +96,13 @@ + + + @@ -181,6 +188,14 @@ + + + + + From 738c3763c28c16f537c0f0305f9cc66531b710ea Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 17 Oct 2023 09:57:42 +0900 Subject: [PATCH 102/206] fix(lane_change): filter out objects out of lane to fix stopping margin chattering (#5321) * fix(lane_change): filter out objects out of lane to fix stopping margin chattering Signed-off-by: kosuke55 * use boost intersects Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../src/scene_module/lane_change/normal.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 769b0d681d2ac..3a2903cb01e9e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -282,6 +282,16 @@ void NormalLaneChange::insertStopPoint( if (v > lane_change_parameters_->stop_velocity_threshold) { return false; } + + // target_objects includes objects out of target lanes, so filter them out + if (!boost::geometry::intersects( + tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + lanelet::utils::combineLaneletsShape(status_.target_lanes) + .polygon2d() + .basicPolygon())) { + return false; + } + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); return stopping_distance_for_obj < distance_to_target_lane_obj && distance_to_target_lane_obj < distance_to_ego_lane_obj; From 99cb32208cd079989f91f69d245d8099c8309eb9 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 17 Oct 2023 12:06:21 +0900 Subject: [PATCH 103/206] feat(avoidance): add another envelope polygon update logic (#5323) feat(avoidance): add other envelope polygon update logic Signed-off-by: satoshi-ota --- .../utils/avoidance/utils.hpp | 3 ++ .../src/utils/avoidance/utils.cpp | 34 +++++++++++++------ 2 files changed, 27 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index d012fd5f612fb..f14e523d0619b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -80,6 +80,9 @@ void setStartData( AvoidLine & al, const double start_shift_length, const geometry_msgs::msg::Pose & start, const size_t start_idx, const double start_dist); +Polygon2d createEnvelopePolygon( + const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer); + Polygon2d createEnvelopePolygon( const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index ed2e42bf0c5ff..232ab5769f0c7 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -475,15 +475,14 @@ void setStartData( } Polygon2d createEnvelopePolygon( - const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) + const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer) { namespace bg = boost::geometry; + using tier4_autoware_utils::expandPolygon; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using Box = bg::model::box; - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); - const auto toPolygon2d = [](const geometry_msgs::msg::Polygon & polygon) { Polygon2d ret{}; @@ -518,11 +517,17 @@ Polygon2d createEnvelopePolygon( tf2::doTransform( toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf); - const auto expanded_polygon = - tier4_autoware_utils::expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); + const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); return expanded_polygon; } +Polygon2d createEnvelopePolygon( + const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer) +{ + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + return createEnvelopePolygon(object_polygon, closest_pose, envelope_buffer); +} + std::vector generateObstaclePolygonsForDrivableArea( const ObjectDataArray & objects, const std::shared_ptr & parameters, const double vehicle_width) @@ -658,8 +663,6 @@ void fillObjectEnvelopePolygon( ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose, const std::shared_ptr & parameters) { - using boost::geometry::within; - const auto object_type = utils::getHighestProbLabel(object_data.object.classification); const auto object_parameter = parameters->object_parameters.at(object_type); @@ -677,15 +680,26 @@ void fillObjectEnvelopePolygon( return; } - const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto envelope_poly = + createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + + if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) { + object_data.envelope_poly = same_id_obj->envelope_poly; + return; + } - if (!within(object_polygon, same_id_obj->envelope_poly)) { + std::vector unions; + boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions); + + if (unions.empty()) { object_data.envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); return; } - object_data.envelope_poly = same_id_obj->envelope_poly; + boost::geometry::correct(unions.front()); + + object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); } void fillObjectMovingTime( From 3f2230f0c734c15f148e3b68a349ca0325be09ee Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 17 Oct 2023 12:12:39 +0900 Subject: [PATCH 104/206] fix(tier4_simulator_launch): add lacked param path (#5326) Signed-off-by: satoshi-ota --- .../launch/simulator.launch.xml | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index aef1e45f517b1..4da6720b0af47 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -65,7 +65,16 @@ - + + + + + + + From 68252008c60103d36b9f1e9d6ee36782968767bc Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 17 Oct 2023 12:19:56 +0900 Subject: [PATCH 105/206] fix(drivable_area_expansion): correct bound limit check (#5325) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 35 +++++++------------ 1 file changed, 12 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 76d1f280651bf..44dddb57e726e 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -126,30 +126,19 @@ std::vector calculate_minimum_expansions( minimum_expansions[bound_idx] = std::max(minimum_expansions[bound_idx], dist); minimum_expansions[bound_idx + 1] = std::max(minimum_expansions[bound_idx + 1], dist); // apply the expansion to all bound points within the extra arc length - if (bound_idx + 2 < bound.size()) { - auto up_arc_length = - tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]) + - tier4_autoware_utils::calcDistance2d(bound[bound_idx + 1], bound[bound_idx + 2]); - for (auto up_bound_idx = bound_idx + 2; - bound_idx < bound.size() && up_arc_length <= params.extra_arc_length; - ++up_bound_idx) { - minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); - if (up_bound_idx + 1 < bound.size()) - up_arc_length += - tier4_autoware_utils::calcDistance2d(bound[up_bound_idx], bound[up_bound_idx + 1]); - } + auto arc_length = + tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); + for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { + tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); } - if (bound_idx > 0) { - auto down_arc_length = - tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]) + - tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); - for (auto down_bound_idx = bound_idx - 1; - down_bound_idx > 0 && down_arc_length <= params.extra_arc_length; --down_bound_idx) { - minimum_expansions[down_bound_idx] = std::max(minimum_expansions[down_bound_idx], dist); - if (down_bound_idx > 1) - down_arc_length += tier4_autoware_utils::calcDistance2d( - bound[down_bound_idx], bound[down_bound_idx - 1]); - } + arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx]); + for (auto down_offset_idx = 1LU; down_offset_idx < bound_idx; ++down_offset_idx) { + const auto idx = bound_idx - down_offset_idx; + arc_length += tier4_autoware_utils::calcDistance2d(bound[idx - 1], bound[idx]); + if (arc_length > params.extra_arc_length) break; + minimum_expansions[idx] = std::max(minimum_expansions[idx], dist); } break; } From 3a1d48076ed7e42fa9e6a47df285979e372f761a Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 17 Oct 2023 13:36:33 +0900 Subject: [PATCH 106/206] feat(imu_corrector): changed GyroBiasEstimator to use ndt pose instead of twist (#5259) * Implemented ndt pose based GyroBiasEstimator Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Added missing includes Signed-off-by: Shintaro Sakoda * FIxed default gyro_bias_threshold Signed-off-by: Shintaro Sakoda * Restored gyro_bias_pub_, which had been deleted due to a mistake Signed-off-by: Shintaro Sakoda * removed get_bias_std and anything else that is no longer needed as a result of remove Signed-off-by: Shintaro Sakoda * Updated README.md Signed-off-by: Shintaro Sakoda * Revert "Updated README.md" This reverts commit f669c20d54eeb471f69edc1e54fabe14ba8a9e16. * Updated README.md Signed-off-by: Shintaro Sakoda * Added notes to README.md Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/imu_corrector/README.md | 31 ++-- .../config/gyro_bias_estimator.param.yaml | 5 +- .../launch/gyro_bias_estimator.launch.xml | 4 +- .../src/gyro_bias_estimation_module.cpp | 124 ++++++++++----- .../src/gyro_bias_estimation_module.hpp | 25 ++- .../imu_corrector/src/gyro_bias_estimator.cpp | 142 +++++++++++++++--- .../imu_corrector/src/gyro_bias_estimator.hpp | 28 +++- .../test/test_gyro_bias_estimation_module.cpp | 76 ++++++---- 8 files changed, 310 insertions(+), 125 deletions(-) diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index b47ae1500f43b..7af82c1129aff 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -51,10 +51,16 @@ Note that the node calculates bias from the gyroscope data by averaging the data ### Input -| Name | Type | Description | -| ----------------- | ------------------------------------------------ | ---------------- | -| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | -| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity | +| Name | Type | Description | +| ----------------- | ----------------------------------------------- | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | ndt pose | + +Note that the input pose is assumed to be accurate enough. For example when using NDT, we assume that the NDT is appropriately converged. + +Currently, it is possible to use methods other than NDT as a `pose_source` for Autoware, but less accurate methods are not suitable for IMU bias estimation. + +In the future, with careful implementation for pose errors, the IMU bias estimated by NDT could potentially be used not only for validation but also for online calibration. ### Output @@ -64,12 +70,11 @@ Note that the node calculates bias from the gyroscope data by averaging the data ### Parameters -| Name | Type | Description | -| --------------------------- | ------ | ----------------------------------------------------------------------------- | -| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | -| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | -| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | -| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | -| `velocity_threshold` | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] | -| `timestamp_threshold` | double | threshold of the timestamp diff between IMU and twist [s] | -| `data_num_threshold` | int | number of data used to calculate bias | +| Name | Type | Description | +| ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- | +| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | +| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | +| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | +| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | +| `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] | +| `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml index d5868e1df416a..e10329c920137 100644 --- a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -1,6 +1,5 @@ /**: ros__parameters: gyro_bias_threshold: 0.0015 # [rad/s] - velocity_threshold: 0.0 # [m/s] - timestamp_threshold: 0.1 # [s] - data_num_threshold: 200 # [num] + timer_callback_interval_sec: 0.5 # [sec] + straight_motion_ang_vel_upper_limit: 0.015 # [rad/s] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml index a25ce5f90ed27..0d7cba9faa3a6 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -1,14 +1,14 @@ - + - + diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp index 2deb6088f6542..9b5719de69524 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -14,58 +14,106 @@ #include "gyro_bias_estimation_module.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + namespace imu_corrector { -GyroBiasEstimationModule::GyroBiasEstimationModule( - const double velocity_threshold, const double timestamp_threshold, - const size_t data_num_threshold) -: velocity_threshold_(velocity_threshold), - timestamp_threshold_(timestamp_threshold), - data_num_threshold_(data_num_threshold), - is_stopped_(false) -{ -} -void GyroBiasEstimationModule::update_gyro( - const double time, const geometry_msgs::msg::Vector3 & gyro) +/** + * @brief perform dead reckoning based on "gyro_list" and return a relative pose (in RPY) + */ +geometry_msgs::msg::Vector3 integrate_orientation( + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) { - if (time - last_velocity_time_ > timestamp_threshold_) { - return; - } - if (!is_stopped_) { - return; - } - gyro_buffer_.push_back(gyro); - if (gyro_buffer_.size() > data_num_threshold_) { - gyro_buffer_.pop_front(); + geometry_msgs::msg::Vector3 d_rpy{}; + double t_prev = rclcpp::Time(gyro_list.front().header.stamp).seconds(); + for (std::size_t i = 0; i < gyro_list.size() - 1; ++i) { + double t_cur = rclcpp::Time(gyro_list[i + 1].header.stamp).seconds(); + + d_rpy.x += (t_cur - t_prev) * (gyro_list[i].vector.x - gyro_bias.x); + d_rpy.y += (t_cur - t_prev) * (gyro_list[i].vector.y - gyro_bias.y); + d_rpy.z += (t_cur - t_prev) * (gyro_list[i].vector.z - gyro_bias.z); + + t_prev = t_cur; } + return d_rpy; } -void GyroBiasEstimationModule::update_velocity(const double time, const double velocity) +/** + * @brief calculate RPY error on dead-reckoning (calculated from "gyro_list") compared to the + * ground-truth pose from "pose_list". + */ +geometry_msgs::msg::Vector3 calculate_error_rpy( + const std::vector & pose_list, + const std::vector & gyro_list, + const geometry_msgs::msg::Vector3 & gyro_bias) { - is_stopped_ = velocity <= velocity_threshold_; - last_velocity_time_ = time; + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_list.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_list.back().pose.orientation); + const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias); + + geometry_msgs::msg::Vector3 error_rpy; + error_rpy.x = tier4_autoware_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x); + error_rpy.y = tier4_autoware_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y); + error_rpy.z = tier4_autoware_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z); + return error_rpy; } -geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const +/** + * @brief update gyroscope bias based on a given trajectory data + */ +void GyroBiasEstimationModule::update_bias( + const std::vector & pose_list, + const std::vector & gyro_list) { - if (gyro_buffer_.size() < data_num_threshold_) { - throw std::runtime_error("Bias estimation is not yet ready because of insufficient data."); + const double dt_pose = + (rclcpp::Time(pose_list.back().header.stamp) - rclcpp::Time(pose_list.front().header.stamp)) + .seconds(); + const double dt_gyro = + (rclcpp::Time(gyro_list.back().header.stamp) - rclcpp::Time(gyro_list.front().header.stamp)) + .seconds(); + if (dt_pose == 0 || dt_gyro == 0) { + throw std::runtime_error("dt_pose or dt_gyro is zero"); } - geometry_msgs::msg::Vector3 bias; - bias.x = 0.0; - bias.y = 0.0; - bias.z = 0.0; - for (const auto & gyro : gyro_buffer_) { - bias.x += gyro.x; - bias.y += gyro.y; - bias.z += gyro.z; + auto error_rpy = calculate_error_rpy(pose_list, gyro_list, geometry_msgs::msg::Vector3{}); + error_rpy.x *= dt_pose / dt_gyro; + error_rpy.y *= dt_pose / dt_gyro; + error_rpy.z *= dt_pose / dt_gyro; + + gyro_bias_pair_.first.x += dt_pose * error_rpy.x; + gyro_bias_pair_.first.y += dt_pose * error_rpy.y; + gyro_bias_pair_.first.z += dt_pose * error_rpy.z; + gyro_bias_pair_.second.x += dt_pose * dt_pose; + gyro_bias_pair_.second.y += dt_pose * dt_pose; + gyro_bias_pair_.second.z += dt_pose * dt_pose; + + geometry_msgs::msg::Vector3 gyro_bias; + gyro_bias.x = error_rpy.x / dt_pose; + gyro_bias.y = error_rpy.y / dt_pose; + gyro_bias.z = error_rpy.z / dt_pose; +} + +/** + * @brief getter function for current estimated bias + */ +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const +{ + geometry_msgs::msg::Vector3 gyro_bias_base; + if ( + gyro_bias_pair_.second.x == 0 || gyro_bias_pair_.second.y == 0 || + gyro_bias_pair_.second.z == 0) { + throw std::runtime_error("gyro_bias_pair_.second is zero"); } - bias.x /= gyro_buffer_.size(); - bias.y /= gyro_buffer_.size(); - bias.z /= gyro_buffer_.size(); - return bias; + gyro_bias_base.x = gyro_bias_pair_.first.x / gyro_bias_pair_.second.x; + gyro_bias_base.y = gyro_bias_pair_.first.y / gyro_bias_pair_.second.y; + gyro_bias_base.z = gyro_bias_pair_.first.z / gyro_bias_pair_.second.z; + return gyro_bias_base; } } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp index 6ebae942fff5d..dfa152d32c0d9 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -11,33 +11,30 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. + #ifndef GYRO_BIAS_ESTIMATION_MODULE_HPP_ #define GYRO_BIAS_ESTIMATION_MODULE_HPP_ -#include +#include +#include #include +#include +#include namespace imu_corrector { class GyroBiasEstimationModule { public: - GyroBiasEstimationModule( - const double velocity_threshold, const double timestamp_threshold, - const size_t data_num_threshold); - geometry_msgs::msg::Vector3 get_bias() const; - void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro); - void update_velocity(const double time, const double velocity); + GyroBiasEstimationModule() = default; + void update_bias( + const std::vector & pose_list, + const std::vector & gyro_list); + geometry_msgs::msg::Vector3 get_bias_base_link() const; private: - const double velocity_threshold_; - const double timestamp_threshold_; - const size_t data_num_threshold_; - bool is_stopped_; - std::deque gyro_buffer_; - - double last_velocity_time_; + std::pair gyro_bias_pair_; }; } // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 5f41a9089b6ee..21bb51dc5f1f1 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -14,6 +14,13 @@ #include "gyro_bias_estimator.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include +#include + +#include + namespace imu_corrector { GyroBiasEstimator::GyroBiasEstimator() @@ -22,59 +29,146 @@ GyroBiasEstimator::GyroBiasEstimator() angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), + timer_callback_interval_sec_(declare_parameter("timer_callback_interval_sec")), + straight_motion_ang_vel_upper_limit_( + declare_parameter("straight_motion_ang_vel_upper_limit")), updater_(this), gyro_bias_(std::nullopt) { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); - const double velocity_threshold = declare_parameter("velocity_threshold"); - const double timestamp_threshold = declare_parameter("timestamp_threshold"); - const size_t data_num_threshold = - static_cast(declare_parameter("data_num_threshold")); - gyro_bias_estimation_module_ = std::make_unique( - velocity_threshold, timestamp_threshold, data_num_threshold); + gyro_bias_estimation_module_ = std::make_unique(); imu_sub_ = create_subscription( "~/input/imu_raw", rclcpp::SensorDataQoS(), [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); - twist_sub_ = create_subscription( - "~/input/twist", rclcpp::SensorDataQoS(), - [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { callback_twist(msg); }); - + pose_sub_ = create_subscription( + "~/input/pose", rclcpp::SensorDataQoS(), + [this](const PoseWithCovarianceStamped::ConstSharedPtr msg) { callback_pose(msg); }); gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); + + auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); + auto period_control = std::chrono::duration_cast( + std::chrono::duration(timer_callback_interval_sec_)); + timer_ = std::make_shared>( + this->get_clock(), period_control, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); + + transform_listener_ = std::make_shared(this); } void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) { - // Update gyro data - gyro_bias_estimation_module_->update_gyro( - rclcpp::Time(imu_msg_ptr->header.stamp).seconds(), imu_msg_ptr->angular_velocity); - - // Estimate gyro bias - try { - gyro_bias_ = gyro_bias_estimation_module_->get_bias(); - } catch (const std::runtime_error & e) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *(this->get_clock()), 1000, e.what()); + imu_frame_ = imu_msg_ptr->header.frame_id; + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = + transform_listener_->getLatestTransform(imu_frame_, output_frame_); + if (!tf_imu2base_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), + (imu_frame_).c_str()); + return; } + geometry_msgs::msg::Vector3Stamped gyro; + gyro.header.stamp = imu_msg_ptr->header.stamp; + gyro.vector = transform_vector3(imu_msg_ptr->angular_velocity, *tf_imu2base_ptr); + + gyro_all_.push_back(gyro); + // Publish results for debugging if (gyro_bias_ != std::nullopt) { Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header.stamp = this->now(); gyro_bias_msg.vector = gyro_bias_.value(); + gyro_bias_pub_->publish(gyro_bias_msg); } +} + +void GyroBiasEstimator::callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr) +{ + // push pose_msg to queue + geometry_msgs::msg::PoseStamped pose; + pose.header = pose_msg_ptr->header; + pose.pose = pose_msg_ptr->pose.pose; + pose_buf_.push_back(pose); +} + +void GyroBiasEstimator::timer_callback() +{ + if (pose_buf_.empty()) { + return; + } + + // Copy data + const std::vector pose_buf = pose_buf_; + const std::vector gyro_all = gyro_all_; + pose_buf_.clear(); + gyro_all_.clear(); + + // Check time + const rclcpp::Time t0_rclcpp_time = rclcpp::Time(pose_buf.front().header.stamp); + const rclcpp::Time t1_rclcpp_time = rclcpp::Time(pose_buf.back().header.stamp); + if (t1_rclcpp_time <= t0_rclcpp_time) { + return; + } + + // Filter gyro data + std::vector gyro_filtered; + for (const auto & gyro : gyro_all) { + const rclcpp::Time t = rclcpp::Time(gyro.header.stamp); + if (t0_rclcpp_time <= t && t < t1_rclcpp_time) { + gyro_filtered.push_back(gyro); + } + } + + // Check gyro data size + // Data size must be greater than or equal to 2 since the time difference will be taken later + if (gyro_filtered.size() <= 1) { + return; + } + + // Check if the vehicle is moving straight + const geometry_msgs::msg::Vector3 rpy_0 = + tier4_autoware_utils::getRPY(pose_buf.front().pose.orientation); + const geometry_msgs::msg::Vector3 rpy_1 = + tier4_autoware_utils::getRPY(pose_buf.back().pose.orientation); + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(rpy_1.z - rpy_0.z)); + const double time_diff = (t1_rclcpp_time - t0_rclcpp_time).seconds(); + const double yaw_vel = yaw_diff / time_diff; + const bool is_straight = (yaw_vel < straight_motion_ang_vel_upper_limit_); + if (!is_straight) { + return; + } + + // Calculate gyro bias + gyro_bias_estimation_module_->update_bias(pose_buf, gyro_filtered); + + geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_base2imu_ptr = + transform_listener_->getLatestTransform(output_frame_, imu_frame_); + if (!tf_base2imu_ptr) { + RCLCPP_ERROR( + this->get_logger(), "Please publish TF %s to %s", imu_frame_.c_str(), output_frame_.c_str()); + return; + } + gyro_bias_ = + transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); - // Update diagnostics updater_.force_update(); } -void GyroBiasEstimator::callback_twist( - const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +geometry_msgs::msg::Vector3 GyroBiasEstimator::transform_vector3( + const geometry_msgs::msg::Vector3 & vec, const geometry_msgs::msg::TransformStamped & transform) { - gyro_bias_estimation_module_->update_velocity( - rclcpp::Time(twist_msg_ptr->header.stamp).seconds(), twist_msg_ptr->twist.twist.linear.x); + geometry_msgs::msg::Vector3Stamped vec_stamped; + vec_stamped.vector = vec; + + geometry_msgs::msg::Vector3Stamped vec_stamped_transformed; + tf2::doTransform(vec_stamped, vec_stamped_transformed, transform); + return vec_stamped_transformed.vector; } void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index e74a0390af3aa..7eb06bcdcb365 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -15,17 +15,19 @@ #define GYRO_BIAS_ESTIMATOR_HPP_ #include "gyro_bias_estimation_module.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include #include -#include +#include #include #include #include #include #include +#include namespace imu_corrector { @@ -33,7 +35,7 @@ class GyroBiasEstimator : public rclcpp::Node { private: using Imu = sensor_msgs::msg::Imu; - using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; using Vector3 = geometry_msgs::msg::Vector3; @@ -43,12 +45,19 @@ class GyroBiasEstimator : public rclcpp::Node private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); - void callback_twist(const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + void callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr); + void timer_callback(); - rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr twist_sub_; + static geometry_msgs::msg::Vector3 transform_vector3( + const geometry_msgs::msg::Vector3 & vec, + const geometry_msgs::msg::TransformStamped & transform); + + const std::string output_frame_ = "base_link"; + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Publisher::SharedPtr gyro_bias_pub_; + rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr gyro_bias_estimation_module_; @@ -56,10 +65,19 @@ class GyroBiasEstimator : public rclcpp::Node const double angular_velocity_offset_x_; const double angular_velocity_offset_y_; const double angular_velocity_offset_z_; + const double timer_callback_interval_sec_; + const double straight_motion_ang_vel_upper_limit_; diagnostic_updater::Updater updater_; std::optional gyro_bias_; + + std::shared_ptr transform_listener_; + + std::string imu_frame_; + + std::vector gyro_all_; + std::vector pose_buf_; }; } // namespace imu_corrector diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp index a0da4d0e24e17..78558feeb7936 100644 --- a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -14,6 +14,8 @@ #include "../src/gyro_bias_estimation_module.hpp" +#include + #include namespace imu_corrector @@ -21,45 +23,67 @@ namespace imu_corrector class GyroBiasEstimationModuleTest : public ::testing::Test { protected: - double velocity_threshold = 1.0; - double timestamp_threshold = 5.0; - size_t data_num_threshold = 10; - GyroBiasEstimationModule module = - GyroBiasEstimationModule(velocity_threshold, timestamp_threshold, data_num_threshold); + GyroBiasEstimationModule module; }; TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) { - geometry_msgs::msg::Vector3 gyro; - gyro.x = 0.1; - gyro.y = 0.2; - gyro.z = 0.3; - for (size_t i = 0; i < data_num_threshold + 1; ++i) { - module.update_velocity( - i * 0.1 * timestamp_threshold, 0.0); // velocity = 0.0 < 1.0 = velocity_threshold - module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + geometry_msgs::msg::PoseStamped pose2; + pose2.header.stamp = rclcpp::Time(1e9); + pose2.pose.orientation.w = 1.0; + pose_list.push_back(pose2); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0.25 * 1e9); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + geometry_msgs::msg::Vector3Stamped gyro2; + gyro2.header.stamp = rclcpp::Time(0.5 * 1e9); + gyro2.vector.x = 0.1; + gyro2.vector.y = 0.2; + gyro2.vector.z = 0.3; + gyro_list.push_back(gyro2); + + for (size_t i = 0; i < 10; ++i) { + module.update_bias(pose_list, gyro_list); } - ASSERT_NEAR(module.get_bias().x, gyro.x, 0.0001); - ASSERT_NEAR(module.get_bias().y, gyro.y, 0.0001); - ASSERT_NEAR(module.get_bias().z, gyro.z, 0.0001); + const geometry_msgs::msg::Vector3 result = module.get_bias_base_link(); + ASSERT_DOUBLE_EQ(result.x, 0.1); + ASSERT_DOUBLE_EQ(result.y, 0.2); + ASSERT_DOUBLE_EQ(result.z, 0.3); } TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) { - ASSERT_THROW(module.get_bias(), std::runtime_error); + ASSERT_THROW(module.get_bias_base_link(), std::runtime_error); } TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) { - geometry_msgs::msg::Vector3 gyro; - gyro.x = 0.1; - gyro.y = 0.2; - gyro.z = 0.3; - for (size_t i = 0; i < data_num_threshold + 1; ++i) { - module.update_velocity( - i * 0.1 * timestamp_threshold, 5.0); // velocity = 5.0 > 1.0 = velocity_threshold - module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + std::vector pose_list; + std::vector gyro_list; + geometry_msgs::msg::PoseStamped pose1; + pose1.header.stamp = rclcpp::Time(0); + pose1.pose.orientation.w = 1.0; + pose_list.push_back(pose1); + + geometry_msgs::msg::Vector3Stamped gyro1; + gyro1.header.stamp = rclcpp::Time(0); + gyro1.vector.x = 0.1; + gyro1.vector.y = 0.2; + gyro1.vector.z = 0.3; + gyro_list.push_back(gyro1); + + for (size_t i = 0; i < 10; ++i) { + ASSERT_THROW(module.update_bias(pose_list, gyro_list), std::runtime_error); } - ASSERT_THROW(module.get_bias(), std::runtime_error); } } // namespace imu_corrector From 0c7ef9863fc95918731abee39375fc7779fd4976 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 17 Oct 2023 15:02:11 +0900 Subject: [PATCH 107/206] feat(ndt_scan_matcher): add initial_to_result_relative_pose (#5319) Added initial_to_result_relative_pose Signed-off-by: Shintaro Sakoda --- localization/ndt_scan_matcher/README.md | 1 + .../ndt_scan_matcher/ndt_scan_matcher_core.hpp | 4 +++- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 13 +++++++++++-- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 1f7779890f89a..e210d3a28796f 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -38,6 +38,7 @@ One optional function is regularization. Please see the regularization chapter i | `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | | `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on de-grounded LiDAR scan | | `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | | `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | | `initial_to_result_distance_old` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | | `initial_to_result_distance_new` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index fc4015aede059..a07eb9cd5c8d0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -111,7 +111,7 @@ class NDTScanMatcher : public rclcpp::Node const pcl::shared_ptr> & sensor_points_in_map_ptr); void publish_marker( const rclcpp::Time & sensor_ros_time, const std::vector & pose_array); - void publish_initial_to_result_distances( + void publish_initial_to_result( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, @@ -150,6 +150,8 @@ class NDTScanMatcher : public rclcpp::Node rclcpp::Publisher::SharedPtr no_ground_nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr + initial_to_result_relative_pose_pub_; rclcpp::Publisher::SharedPtr initial_to_result_distance_pub_; rclcpp::Publisher::SharedPtr diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 238e5c6be89bc..25033c8c83c01 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -205,6 +205,8 @@ NDTScanMatcher::NDTScanMatcher() "no_ground_nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = this->create_publisher("iteration_num", 10); + initial_to_result_relative_pose_pub_ = + this->create_publisher("initial_to_result_relative_pose", 10); initial_to_result_distance_pub_ = this->create_publisher("initial_to_result_distance", 10); initial_to_result_distance_old_pub_ = @@ -487,7 +489,7 @@ void NDTScanMatcher::callback_sensor_points( publish_tf(sensor_ros_time, result_pose_msg); publish_pose(sensor_ros_time, result_pose_msg, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); - publish_initial_to_result_distances( + publish_initial_to_result( sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), interpolator.get_new_pose()); @@ -628,12 +630,19 @@ void NDTScanMatcher::publish_marker( ndt_marker_pub_->publish(marker_array); } -void NDTScanMatcher::publish_initial_to_result_distances( +void NDTScanMatcher::publish_initial_to_result( const rclcpp::Time & sensor_ros_time, const geometry_msgs::msg::Pose & result_pose_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_cov_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_old_msg, const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_new_msg) { + geometry_msgs::msg::PoseStamped initial_to_result_relative_pose_stamped; + initial_to_result_relative_pose_stamped.pose = + tier4_autoware_utils::inverseTransformPose(result_pose_msg, initial_pose_cov_msg.pose.pose); + initial_to_result_relative_pose_stamped.header.stamp = sensor_ros_time; + initial_to_result_relative_pose_stamped.header.frame_id = map_frame_; + initial_to_result_relative_pose_pub_->publish(initial_to_result_relative_pose_stamped); + const auto initial_to_result_distance = static_cast(norm(initial_pose_cov_msg.pose.pose.position, result_pose_msg.position)); initial_to_result_distance_pub_->publish( From d38beae3097fe614806f0b900b7b6417291c5c10 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 17 Oct 2023 17:27:32 +0900 Subject: [PATCH 108/206] fix(tracking_object_merger): fix unintended error in radar tracking merger (#5328) * fix: fix tracking merger node Signed-off-by: yoshiri * fix: unintended condition setting Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../camera_lidar_radar_fusion_based_detection.launch.xml | 2 +- .../include/tracking_object_merger/utils/utils.hpp | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 7d0988635fb10..07640495a5e19 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -411,7 +411,7 @@ - + diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp index 013040d15bded..bc6dfef9b80bf 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/utils.hpp @@ -27,8 +27,12 @@ #include #include #include +#include #include +#include +#include +#include #include #include From 08f6b8de901ca70df1705d420df0b596958e0794 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 17 Oct 2023 17:54:34 +0900 Subject: [PATCH 109/206] chore(autonmous_emergency_braking): add maintainer (#5331) Signed-off-by: tomoya.kimura --- control/autonomous_emergency_braking/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 4f2ad6c50a9b3..5dc65cb243bfc 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -6,6 +6,8 @@ Autonomous Emergency Braking package as a ROS 2 node Yutaka Shimizu Takamasa Horibe + Tomoya Kimura + Apache License 2.0 ament_cmake From 076319bef0e92f23497b05be1e7f9e8de9bdebef Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Tue, 17 Oct 2023 09:08:14 +0000 Subject: [PATCH 110/206] chore: update CODEOWNERS (#5184) Signed-off-by: GitHub Co-authored-by: github-actions --- .github/CODEOWNERS | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index f9983f0fccc3f..9de27f82ffcef 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -56,7 +56,7 @@ common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.j common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp +control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@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 control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp control/external_cmd_selector/** 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 @@ -66,6 +66,7 @@ control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier control/obstacle_collision_checker/** 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 control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/pid_longitudinal_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -140,6 +141,7 @@ perception/simple_object_merger/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4. perception/tensorrt_classifier/** mingyu.li@tier4.jp perception/tensorrt_yolo/** daisuke.nishimatsu@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/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 @@ -181,7 +183,6 @@ planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp -planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp @@ -215,6 +216,7 @@ system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhoo system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/dummy_diag_publisher/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp +system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp From 8453525fcc9dace38b0780d791e890ce2a2461b1 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 17 Oct 2023 18:36:20 +0900 Subject: [PATCH 111/206] fix(AEB): failure due to an empty path (#5329) Signed-off-by: Takamasa Horibe Co-authored-by: Tomoya Kimura --- control/autonomous_emergency_braking/src/node.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index e68dff6b0ec1d..0ccc9bc4a3dae 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -432,6 +432,10 @@ void AEB::generateEgoPath( const Trajectory & predicted_traj, Path & path, std::vector & polygons) { + if (predicted_traj.points.empty()) { + return; + } + geometry_msgs::msg::TransformStamped transform_stamped{}; try { transform_stamped = tf_buffer_.lookupTransform( From b71b04e52e62eb23b2affb6fdcf1bab59b1de298 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 17 Oct 2023 19:08:42 +0900 Subject: [PATCH 112/206] feat(avoidance): make detection area dynamically (#5303) Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 9 +++++++-- .../utils/avoidance/avoidance_module_data.hpp | 6 +++--- .../utils/avoidance/helper.hpp | 16 ++++++++++++++++ .../utils/avoidance/utils.hpp | 2 +- .../scene_module/avoidance/avoidance_module.cpp | 7 ++++--- .../src/scene_module/avoidance/manager.cpp | 15 +++++++++++---- .../src/scene_module/lane_change/manager.cpp | 15 +++++++++++---- .../src/utils/avoidance/utils.cpp | 8 ++++---- 8 files changed, 57 insertions(+), 21 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 2b38536341d7c..25e42d20d144a 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -118,8 +118,6 @@ object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] object_check_goal_distance: 20.0 # [m] # filtering parking objects threshold_distance_object_is_on_center: 1.0 # [m] @@ -128,6 +126,13 @@ # lost object compensation object_last_seen_threshold: 2.0 + # detection area generation parameters + detection_area: + static: true # [-] + min_forward_distance: 50.0 # [m] + max_forward_distance: 150.0 # [m] + backward_distance: 10.0 # [m] + # For safety check safety_check: # safety check configuration diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 5ba0085ded5b7..d04b35a3b185b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -145,9 +145,9 @@ struct AvoidanceParameters double object_ignore_section_crosswalk_behind_distance{0.0}; // distance to avoid object detection - double object_check_forward_distance{0.0}; - - // continue to detect backward vehicles as avoidance targets until they are this distance away + bool use_static_detection_area{true}; + double object_check_min_forward_distance{0.0}; + double object_check_max_forward_distance{0.0}; double object_check_backward_distance{0.0}; // if the distance between object and goal position is less than this parameter, the module ignore diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 77fe2e29c4978..11388dd6bb74a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -173,6 +173,22 @@ class AvoidanceHelper : std::max(shift_length, getRightShiftBound()); } + double getForwardDetectionRange() const + { + if (parameters_->use_static_detection_area) { + return parameters_->object_check_max_forward_distance; + } + + const auto max_shift_length = std::max( + std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length)); + const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk( + max_shift_length, getLateralMinJerkLimit(), getEgoSpeed()); + + return std::clamp( + 1.5 * dynamic_distance, parameters_->object_check_min_forward_distance, + parameters_->object_check_max_forward_distance); + } + void alignShiftLinesOrder(AvoidLineArray & lines, const bool align_shift_length = true) const { if (lines.empty()) { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index f14e523d0619b..8cb09cd47b444 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -167,7 +167,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const bool is_running, DebugData & debug); + const double object_check_forward_distance, const bool is_running, DebugData & debug); DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index c94406e466219..b4b0a871eec62 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -322,11 +322,12 @@ void AvoidanceModule::fillAvoidanceTargetObjects( getCurrentStatus() == ModuleStatus::WAITING_APPROVAL; // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. + const auto sparse_resample_path = utils::resamplePathWithSpline( + helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); const auto [object_within_target_lane, object_outside_target_lane] = utils::avoidance::separateObjectsByPath( - utils::resamplePathWithSpline( - helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output), - planner_data_, data, parameters_, is_running, debug); + sparse_resample_path, planner_data_, data, parameters_, helper_.getForwardDetectionRange(), + is_running, debug); for (const auto & object : object_outside_target_lane.objects) { ObjectData other_object; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 500ebe873a2d8..763cf17890721 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -114,10 +114,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - getOrDeclareParameter(*node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "object_check_backward_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -130,6 +126,17 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + // safety check general params { std::string ns = "avoidance.safety_check."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 3d7cf07308a79..c6995ada1bfa6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -329,10 +329,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( *node, ns + "object_ignore_section_crosswalk_in_front_distance"); p.object_ignore_section_crosswalk_behind_distance = getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); - p.object_check_forward_distance = - getOrDeclareParameter(*node, ns + "object_check_forward_distance"); - p.object_check_backward_distance = - getOrDeclareParameter(*node, ns + "object_check_backward_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -345,6 +341,17 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.detection_area."; + p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); + p.object_check_min_forward_distance = + getOrDeclareParameter(*node, ns + "min_forward_distance"); + p.object_check_max_forward_distance = + getOrDeclareParameter(*node, ns + "max_forward_distance"); + p.object_check_backward_distance = + getOrDeclareParameter(*node, ns + "backward_distance"); + } + // safety check { std::string ns = "avoidance.safety_check."; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 232ab5769f0c7..8fb30a28f1272 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -991,7 +991,7 @@ void filterTargetObjects( data.other_objects.push_back(o); continue; } - if (o.longitudinal > parameters->object_check_forward_distance) { + if (o.longitudinal > parameters->object_check_max_forward_distance) { o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; data.other_objects.push_back(o); continue; @@ -1479,7 +1479,7 @@ lanelet::ConstLanelets getAdjacentLane( const std::shared_ptr & parameters, const bool is_right_shift) { const auto & rh = planner_data->route_handler; - const auto & forward_distance = parameters->object_check_forward_distance; + const auto & forward_distance = parameters->object_check_max_forward_distance; const auto & backward_distance = parameters->safety_check_backward_distance; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; @@ -1619,7 +1619,7 @@ std::vector getSafetyCheckTargetObjects( std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, - const bool is_running, DebugData & debug) + const double object_check_forward_distance, const bool is_running, DebugData & debug) { PredictedObjects target_objects; PredictedObjects other_objects; @@ -1642,7 +1642,7 @@ std::pair separateObjectsByPath( const auto & p_ego_back = path.points.at(i + 1).point.pose; const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i); - if (distance_from_ego > parameters->object_check_forward_distance) { + if (distance_from_ego > object_check_forward_distance) { break; } From 86a59542d1ef6ec24e168832f9c35bfa2af66e73 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Tue, 17 Oct 2023 21:55:26 +0900 Subject: [PATCH 113/206] fix(drivable_area_expansion): fix max_arc_length parameter and extra_arc_length parameters (#5333) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.cpp | 29 ++++++++++++------- 1 file changed, 19 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 44dddb57e726e..25cf917d27135 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -74,14 +74,21 @@ void reuse_previous_poses( if (cropped_poses.empty()) { const auto resampled_path_points = motion_utils::resamplePath(path, params.resample_interval, true, true, false).points; - for (const auto & p : resampled_path_points) cropped_poses.push_back(p.point.pose); - } else if (!path.points.empty()) { + const auto cropped_path = + params.max_path_arc_length <= 0.0 + ? resampled_path_points + : motion_utils::cropForwardPoints( + resampled_path_points, resampled_path_points.front().point.pose.position, 0, + params.max_path_arc_length); + for (const auto & p : cropped_path) cropped_poses.push_back(p.point.pose); + } else { const auto initial_arc_length = motion_utils::calcArcLength(cropped_poses); const auto max_path_arc_length = motion_utils::calcArcLength(path.points); const auto first_arc_length = motion_utils::calcSignedArcLength( path.points, path.points.front().point.pose.position, cropped_poses.back().position); for (auto arc_length = first_arc_length + params.resample_interval; - initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length && + (params.max_path_arc_length <= 0.0 || + initial_arc_length + (arc_length - first_arc_length) <= params.max_path_arc_length) && arc_length <= max_path_arc_length; arc_length += params.resample_interval) cropped_poses.push_back(motion_utils::calcInterpolatedPose(path.points, arc_length)); @@ -129,7 +136,8 @@ std::vector calculate_minimum_expansions( auto arc_length = tier4_autoware_utils::calcDistance2d(*intersection_point, bound[bound_idx + 1]); for (auto up_bound_idx = bound_idx + 2; up_bound_idx < bound.size(); ++up_bound_idx) { - tier4_autoware_utils::calcDistance2d(bound[bound_idx - 1], bound[bound_idx]); + arc_length += + tier4_autoware_utils::calcDistance2d(bound[up_bound_idx - 1], bound[up_bound_idx]); if (arc_length > params.extra_arc_length) break; minimum_expansions[up_bound_idx] = std::max(minimum_expansions[up_bound_idx], dist); } @@ -200,11 +208,11 @@ void expand_bound( LineString2d path_ls; for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); for (auto idx = 0LU; idx < bound.size(); ++idx) { - const auto bound_p = convert_point(bound[idx]); - const auto projection = point_to_linestring_projection(bound_p, path_ls); - const auto expansion_ratio = - (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); - if (expansion_ratio > 1.0) { + if (expansions[idx] > 0.0) { + const auto bound_p = convert_point(bound[idx]); + const auto projection = point_to_linestring_projection(bound_p, path_ls); + const auto expansion_ratio = + (expansions[idx] + std::abs(projection.distance)) / std::abs(projection.distance); const auto & path_p = projection.projected_point; const auto expanded_p = lerp_point(path_p, bound_p, expansion_ratio); bound[idx].x = expanded_p.x(); @@ -267,6 +275,7 @@ void expand_drivable_area( stop_watch.tic("crop"); std::vector path_poses = planner_data->drivable_area_expansion_prev_path_poses; std::vector curvatures = planner_data->drivable_area_expansion_prev_curvatures; + reuse_previous_poses( path, path_poses, curvatures, planner_data->self_odometry->pose.pose.position, params); const auto crop_ms = stop_watch.toc("crop"); @@ -299,6 +308,7 @@ void expand_drivable_area( apply_bound_change_rate_limit(left_expansions, path.left_bound, params.max_bound_rate); apply_bound_change_rate_limit(right_expansions, path.right_bound, params.max_bound_rate); const auto smooth_ms = stop_watch.toc("smooth"); + // TODO(Maxime): limit the distances based on the total width (left + right < min_lane_width) stop_watch.tic("expand"); expand_bound(path.left_bound, path_poses, left_expansions); @@ -312,7 +322,6 @@ void expand_drivable_area( "%2.2f\n\tMaximum expansion: %2.2f\n\tSmoothing: %2.2f\n\tExpansion: %2.2f\n\n", total_ms, preprocessing_ms, crop_ms, curvature_expansion_ms, max_dist_ms, smooth_ms, expand_ms); - planner_data->drivable_area_expansion_prev_path_poses = path_poses; planner_data->drivable_area_expansion_prev_curvatures = curvatures; } From c65bccd981cae83eac8f4b85d688d297db49ad1b Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 17 Oct 2023 22:04:11 +0900 Subject: [PATCH 114/206] refactor(map_based_prediction): to improve readability. Possibly (unlikely) performance improvements (#5261) * refactor to improve readability. Possibly (unlikely) performance Signed-off-by: Daniel Sanchez * Use ternary operator to make the code more readable Signed-off-by: Daniel Sanchez Co-authored-by: Takamasa Horibe * Add const to curr lanelets and pred. object crosswalk Signed-off-by: Daniel Sanchez * remove redundant breaks in switch table Signed-off-by: Daniel Sanchez * change magic numbers to autoware function kph -> mps Signed-off-by: Daniel Sanchez * simplify code by returning on false condition, change ifelse to switch Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez Co-authored-by: Takamasa Horibe --- .../src/map_based_prediction_node.cpp | 322 +++++++++--------- 1 file changed, 156 insertions(+), 166 deletions(-) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 58356f4f96ed6..745bccf6dbabe 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -369,17 +370,13 @@ bool withinLanelet( boost::geometry::correct(polygon); bool with_in_polygon = boost::geometry::within(p_object, polygon); - if (!use_yaw_information) { - return with_in_polygon; - } else { - // use yaw angle to compare - const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); - if (abs_yaw_diff < yaw_threshold) { - return with_in_polygon; - } else { - return false; - } - } + if (!use_yaw_information) return with_in_polygon; + + // use yaw angle to compare + const double abs_yaw_diff = calcAbsYawDiffBetweenLaneletAndObject(object, lanelet); + if (abs_yaw_diff < yaw_threshold) return with_in_polygon; + + return false; } bool withinRoadLanelet( @@ -594,53 +591,53 @@ ObjectClassification::_label_type changeLabelForPrediction( const lanelet::LaneletMapPtr & lanelet_map_ptr_) { // for car like vehicle do not change labels - if ( - label == ObjectClassification::CAR || label == ObjectClassification::BUS || - label == ObjectClassification::TRUCK || label == ObjectClassification::TRAILER || - label == ObjectClassification::UNKNOWN) { - return label; - } else if ( // for bicycle and motorcycle - label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE) { - // if object is within road lanelet and satisfies yaw constraints - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float high_speed_threshold = 25.0 / 18.0 * 5.0; // High speed bicycle 25 km/h - // calc abs speed from x and y velocity - const double abs_speed = std::hypot( - object.kinematics.twist_with_covariance.twist.linear.x, - object.kinematics.twist_with_covariance.twist.linear.y); - const bool high_speed_object = abs_speed > high_speed_threshold; - - // if the object is within lanelet, do the same estimation with vehicle - if (within_road_lanelet) { - return ObjectClassification::MOTORCYCLE; - } else if (high_speed_object) { + switch (label) { + case ObjectClassification::CAR: + case ObjectClassification::BUS: + case ObjectClassification::TRUCK: + case ObjectClassification::TRAILER: + case ObjectClassification::UNKNOWN: + return label; + + case ObjectClassification::MOTORCYCLE: + case ObjectClassification::BICYCLE: { // if object is within road lanelet and satisfies yaw + // constraints + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float high_speed_threshold = + tier4_autoware_utils::kmph2mps(25.0); // High speed bicycle 25 km/h + // calc abs speed from x and y velocity + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > high_speed_threshold; + + // if the object is within lanelet, do the same estimation with vehicle + if (within_road_lanelet) return ObjectClassification::MOTORCYCLE; // high speed object outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; // temporary disabled - return label; - } else { + if (high_speed_object) return label; // Do nothing for now return ObjectClassification::BICYCLE; } - } else if (label == ObjectClassification::PEDESTRIAN) { - const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); - const float max_velocity_for_human_mps = - 25.0 / 18.0 * 5.0; // Max human being motion speed is 25km/h - const double abs_speed = std::hypot( - object.kinematics.twist_with_covariance.twist.linear.x, - object.kinematics.twist_with_covariance.twist.linear.y); - const bool high_speed_object = abs_speed > max_velocity_for_human_mps; - // fast, human-like object: like segway - if (within_road_lanelet && high_speed_object) { - return label; // currently do nothing + + case ObjectClassification::PEDESTRIAN: { + const bool within_road_lanelet = withinRoadLanelet(object, lanelet_map_ptr_, true); + const float max_velocity_for_human_mps = + tier4_autoware_utils::kmph2mps(25.0); // Max human being motion speed is 25km/h + const double abs_speed = std::hypot( + object.kinematics.twist_with_covariance.twist.linear.x, + object.kinematics.twist_with_covariance.twist.linear.y); + const bool high_speed_object = abs_speed > max_velocity_for_human_mps; + // fast, human-like object: like segway + if (within_road_lanelet && high_speed_object) return label; // currently do nothing // return ObjectClassification::MOTORCYCLE; - } else if (high_speed_object) { - return label; // currently do nothing + if (high_speed_object) return label; // currently do nothing // fast human outside road lanelet will move like unknown object // return ObjectClassification::UNKNOWN; - } else { return label; } - } else { - return label; + + default: + return label; } } @@ -850,102 +847,99 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt const auto & label_ = transformed_object.classification.front().label; const auto label = changeLabelForPrediction(label_, object, lanelet_map_ptr_); - // For crosswalk user - if (label == ObjectClassification::PEDESTRIAN || label == ObjectClassification::BICYCLE) { - const auto predicted_object = getPredictedObjectAsCrosswalkUser(transformed_object); - output.objects.push_back(predicted_object); - // For road user - } else if ( - label == ObjectClassification::CAR || label == ObjectClassification::BUS || - label == ObjectClassification::TRAILER || label == ObjectClassification::MOTORCYCLE || - label == ObjectClassification::TRUCK) { - // Update object yaw and velocity - updateObjectData(transformed_object); - - // Get Closest Lanelet - const auto current_lanelets = getCurrentLanelets(transformed_object); - - // Update Objects History - updateObjectsHistory(output.header, transformed_object, current_lanelets); - - // For off lane obstacles - if (current_lanelets.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForOffLaneVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; - } - - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; + switch (label) { + case ObjectClassification::PEDESTRIAN: + case ObjectClassification::BICYCLE: { + const auto predicted_object_crosswalk = + getPredictedObjectAsCrosswalkUser(transformed_object); + output.objects.push_back(predicted_object_crosswalk); + break; } - - // For too-slow vehicle - const double abs_obj_speed = std::hypot( - transformed_object.kinematics.twist_with_covariance.twist.linear.x, - transformed_object.kinematics.twist_with_covariance.twist.linear.y); - if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; + case ObjectClassification::CAR: + case ObjectClassification::BUS: + case ObjectClassification::TRAILER: + case ObjectClassification::MOTORCYCLE: + case ObjectClassification::TRUCK: { + // Update object yaw and velocity + updateObjectData(transformed_object); + + // Get Closest Lanelet + const auto current_lanelets = getCurrentLanelets(transformed_object); + + // Update Objects History + updateObjectsHistory(output.header, transformed_object, current_lanelets); + + // For off lane obstacles + if (current_lanelets.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForOffLaneVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_object_vehicle = convertToPredictedObject(transformed_object); + predicted_object_vehicle.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object_vehicle); + break; } - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; - } - - // Get Predicted Reference Path for Each Maneuver and current lanelets - // return: - const auto ref_paths = - getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); - - // If predicted reference path is empty, assume this object is out of the lane - if (ref_paths.empty()) { - PredictedPath predicted_path = - path_generator_->generatePathForLowSpeedVehicle(transformed_object); - predicted_path.confidence = 1.0; - if (predicted_path.path.empty()) { - continue; + // For too-slow vehicle + const double abs_obj_speed = std::hypot( + transformed_object.kinematics.twist_with_covariance.twist.linear.x, + transformed_object.kinematics.twist_with_covariance.twist.linear.y); + if (std::fabs(abs_obj_speed) < min_velocity_for_map_based_prediction_) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_slow_object = convertToPredictedObject(transformed_object); + predicted_slow_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_slow_object); + break; } - auto predicted_object = convertToPredictedObject(transformed_object); - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); - continue; - } + // Get Predicted Reference Path for Each Maneuver and current lanelets + // return: + const auto ref_paths = + getPredictedReferencePath(transformed_object, current_lanelets, objects_detected_time); + + // If predicted reference path is empty, assume this object is out of the lane + if (ref_paths.empty()) { + PredictedPath predicted_path = + path_generator_->generatePathForLowSpeedVehicle(transformed_object); + predicted_path.confidence = 1.0; + if (predicted_path.path.empty()) break; + + auto predicted_object_out_of_lane = convertToPredictedObject(transformed_object); + predicted_object_out_of_lane.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_object_out_of_lane); + break; + } - // Get Debug Marker for On Lane Vehicles - const auto max_prob_path = std::max_element( - ref_paths.begin(), ref_paths.end(), - [](const PredictedRefPath & a, const PredictedRefPath & b) { - return a.probability < b.probability; - }); - const auto debug_marker = - getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); - debug_markers.markers.push_back(debug_marker); - - // Generate Predicted Path - std::vector predicted_paths; - for (const auto & ref_path : ref_paths) { - PredictedPath predicted_path = - path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); - if (predicted_path.path.empty()) { - continue; + // Get Debug Marker for On Lane Vehicles + const auto max_prob_path = std::max_element( + ref_paths.begin(), ref_paths.end(), + [](const PredictedRefPath & a, const PredictedRefPath & b) { + return a.probability < b.probability; + }); + const auto debug_marker = + getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); + debug_markers.markers.push_back(debug_marker); + + // Generate Predicted Path + std::vector predicted_paths; + for (const auto & ref_path : ref_paths) { + PredictedPath predicted_path = + path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + if (predicted_path.path.empty()) { + continue; + } + predicted_path.confidence = ref_path.probability; + predicted_paths.push_back(predicted_path); } - predicted_path.confidence = ref_path.probability; - predicted_paths.push_back(predicted_path); - } + // Normalize Path Confidence and output the predicted object - // Normalize Path Confidence and output the predicted object - { float sum_confidence = 0.0; for (const auto & predicted_path : predicted_paths) { sum_confidence += predicted_path.confidence; @@ -953,25 +947,25 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt const float min_sum_confidence_value = 1e-3; sum_confidence = std::max(sum_confidence, min_sum_confidence_value); + auto predicted_object = convertToPredictedObject(transformed_object); + for (auto & predicted_path : predicted_paths) { predicted_path.confidence = predicted_path.confidence / sum_confidence; - } - - auto predicted_object = convertToPredictedObject(transformed_object); - for (const auto & predicted_path : predicted_paths) { predicted_object.kinematics.predicted_paths.push_back(predicted_path); } output.objects.push_back(predicted_object); + break; } - // For unknown object - } else { - auto predicted_object = convertToPredictedObject(transformed_object); - PredictedPath predicted_path = - path_generator_->generatePathForNonVehicleObject(transformed_object); - predicted_path.confidence = 1.0; + default: { + auto predicted_unknown_object = convertToPredictedObject(transformed_object); + PredictedPath predicted_path = + path_generator_->generatePathForNonVehicleObject(transformed_object); + predicted_path.confidence = 1.0; - predicted_object.kinematics.predicted_paths.push_back(predicted_path); - output.objects.push_back(predicted_object); + predicted_unknown_object.kinematics.predicted_paths.push_back(predicted_path); + output.objects.push_back(predicted_unknown_object); + break; + } } } @@ -1114,26 +1108,28 @@ void MapBasedPredictionNode::updateObjectData(TrackedObject & object) object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0); // assumption: the object vx is much larger than vy - if (object.kinematics.twist_with_covariance.twist.linear.x < 0.0) { - if ( - object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + if (object.kinematics.twist_with_covariance.twist.linear.x >= 0.0) return; + + switch (object.kinematics.orientation_availability) { + case autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN: { const auto original_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); // flip the angle object.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(tier4_autoware_utils::pi + original_yaw); - } else { + break; + } + default: { const auto updated_object_yaw = tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position); object.kinematics.pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(updated_object_yaw); + break; } - - object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; - object.kinematics.twist_with_covariance.twist.linear.y *= -1.0; } + object.kinematics.twist_with_covariance.twist.linear.x *= -1.0; + object.kinematics.twist_with_covariance.twist.linear.y *= -1.0; return; } @@ -1864,16 +1860,10 @@ ManeuverProbability MapBasedPredictionNode::calculateManeuverProbability( constexpr float LC_PROB = 1.0; // probability for left lane change constexpr float RC_PROB = 1.0; // probability for right lane change lane_follow_probability = 0.0; - if (!left_paths.empty() && right_paths.empty()) { - left_lane_change_probability = LC_PROB; - right_lane_change_probability = 0.0; - } else if (left_paths.empty() && !right_paths.empty()) { - left_lane_change_probability = 0.0; - right_lane_change_probability = RC_PROB; - } else { - left_lane_change_probability = LC_PROB; - right_lane_change_probability = RC_PROB; - } + + // If the given lane is empty, the probability goes to 0 + left_lane_change_probability = left_paths.empty() ? 0.0 : LC_PROB; + right_lane_change_probability = right_paths.empty() ? 0.0 : RC_PROB; } const float MIN_PROBABILITY = 1e-3; From b3bdedc584682fe6ab6310af4792312c36521e74 Mon Sep 17 00:00:00 2001 From: beyzanurkaya <32412808+beyzanurkaya@users.noreply.github.com> Date: Wed, 18 Oct 2023 00:40:26 +0300 Subject: [PATCH 115/206] fix(obstacle_avoidance_planner): fix lateral calculations (#4292) * use right_overhang and left_overhang for lateral calculation Signed-off-by: beyza * style(pre-commit): autofix * update debug_marker.cpp * style(pre-commit): autofix --------- Signed-off-by: beyza Co-authored-by: beyza Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/debug_marker.cpp | 56 +++++++++++-------- .../src/mpt_optimizer.cpp | 17 ++++-- 2 files changed, 43 insertions(+), 30 deletions(-) diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index 8bf46c697104a..2f8babf103877 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -47,21 +47,22 @@ MarkerArray getFootprintsMarkerArray( const auto & traj_point = mpt_traj.at(i); - const double half_width = vehicle_info.vehicle_width_m / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; const double base_to_rear = vehicle_info.rear_overhang_m; marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, base_to_left, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, base_to_front, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -base_to_right, 0.0) .position); marker.points.push_back( - tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, -half_width, 0.0) + tier4_autoware_utils::calcOffsetPose(traj_point.pose, -base_to_rear, base_to_left, 0.0) .position); marker.points.push_back(marker.points.front()); @@ -71,8 +72,8 @@ MarkerArray getFootprintsMarkerArray( } MarkerArray getBoundsWidthMarkerArray( - const std::vector & ref_points, const double vehicle_width, - const size_t sampling_num) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); MarkerArray marker_array; @@ -105,8 +106,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; const double lb_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(pose.position); @@ -125,8 +128,10 @@ MarkerArray getBoundsWidthMarkerArray( } const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose_on_constraints.at(bound_idx); + const double base_to_left = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const double ub_y = - ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + vehicle_width / 2.0; + ref_points.at(i).bounds_on_constraints.at(bound_idx).upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(pose.position); @@ -140,7 +145,8 @@ MarkerArray getBoundsWidthMarkerArray( } MarkerArray getBoundsLineMarkerArray( - const std::vector & ref_points, const double vehicle_width) + const std::vector & ref_points, + const vehicle_info_util::VehicleInfo & vehicle_info) { MarkerArray marker_array; @@ -158,12 +164,13 @@ MarkerArray getBoundsLineMarkerArray( for (size_t i = 0; i < ref_points.size(); i++) { const geometry_msgs::msg::Pose & pose = ref_points.at(i).pose; - - const double ub_y = ref_points.at(i).bounds.upper_bound + vehicle_width / 2.0; + const double base_to_right = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; + const double ub_y = ref_points.at(i).bounds.upper_bound + base_to_left; const auto ub = tier4_autoware_utils::calcOffsetPose(pose, 0.0, ub_y, 0.0).position; ub_marker.points.push_back(ub); - const double lb_y = ref_points.at(i).bounds.lower_bound - vehicle_width / 2.0; + const double lb_y = ref_points.at(i).bounds.lower_bound - base_to_right; const auto lb = tier4_autoware_utils::calcOffsetPose(pose, 0.0, lb_y, 0.0).position; lb_marker.points.push_back(lb); } @@ -175,8 +182,9 @@ MarkerArray getBoundsLineMarkerArray( MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, - const std::vector & vehicle_circle_longitudinal_offsets, const double vehicle_width, - const size_t sampling_num, const std::string & ns) + const std::vector & vehicle_circle_longitudinal_offsets, + const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num, + const std::string & ns) { const auto current_time = rclcpp::Clock().now(); MarkerArray msg; @@ -206,11 +214,13 @@ MarkerArray getVehicleCircleLinesMarkerArray( auto base_pose = tier4_autoware_utils::calcOffsetPose(pose_with_deviation, d, 0.0, 0.0); base_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(ref_point.getYaw() + ref_point.alpha); - + const double base_to_right = + (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.right_overhang_m; + const double base_to_left = (vehicle_info.wheel_tread_m / 2.0) + vehicle_info.left_overhang_m; const auto ub = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, base_to_left, 0.0).position; const auto lb = - tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -vehicle_width / 2.0, 0.0).position; + tier4_autoware_utils::calcOffsetPose(base_pose, 0.0, -base_to_right, 0.0).position; marker.points.push_back(ub); marker.points.push_back(lb); @@ -368,13 +378,12 @@ MarkerArray getDebugMarker( MarkerArray marker_array; // bounds line - appendMarkerArray( - getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info.vehicle_width_m), &marker_array); + appendMarkerArray(getBoundsLineMarkerArray(debug_data.ref_points, vehicle_info), &marker_array); // bounds width appendMarkerArray( getBoundsWidthMarkerArray( - debug_data.ref_points, vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num), + debug_data.ref_points, vehicle_info, debug_data.mpt_visualize_sampling_num), &marker_array); // current vehicle circles @@ -404,9 +413,8 @@ MarkerArray getDebugMarker( // vehicle circle line appendMarkerArray( getVehicleCircleLinesMarkerArray( - debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, - vehicle_info.vehicle_width_m, debug_data.mpt_visualize_sampling_num, - "vehicle_circle_lines"), + debug_data.ref_points, debug_data.vehicle_circle_longitudinal_offsets, vehicle_info, + debug_data.mpt_visualize_sampling_num, "vehicle_circle_lines"), &marker_array); // footprint by drivable area diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 359485fadcfff..765136b3cf6f6 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -36,9 +36,11 @@ std::tuple, std::vector> calcVehicleCirclesByUniform const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, const double radius_ratio) { + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; const double radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * radius_ratio; const std::vector radiuses(circle_num, radius); @@ -59,16 +61,18 @@ std::tuple, std::vector> calcVehicleCirclesByBicycle if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; // 1st circle (rear wheel) - const double rear_radius = vehicle_info.vehicle_width_m / 2.0 * rear_radius_ratio; + const double rear_radius = + vehicle_info.vehicle_width_m / 2.0 + lateral_offset * rear_radius_ratio; const double rear_lon_offset = 0.0; // 2nd circle (front wheel) const double front_radius = std::hypot( vehicle_info.vehicle_length_m / static_cast(circle_num) / 2.0, - vehicle_info.vehicle_width_m / 2.0) * + vehicle_info.vehicle_width_m / 2.0 + lateral_offset) * front_radius_ratio; const double unit_lon_length = vehicle_info.vehicle_length_m / static_cast(circle_num); @@ -84,8 +88,9 @@ std::tuple, std::vector> calcVehicleCirclesByFitting if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); } - - const double radius = vehicle_info.vehicle_width_m / 2.0; + const double lateral_offset = + abs(vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; + const double radius = vehicle_info.vehicle_width_m / 2.0 + lateral_offset; std::vector radiuses(circle_num, radius); const double unit_lon_length = From 5c220226e06f94416ee7794ae40a165a76680799 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 18 Oct 2023 09:54:51 +0900 Subject: [PATCH 116/206] fix(lane_change): detect stuck in current lane termianl (#5337) * feat(lane_change): detect stuck in current lane termianl Signed-off-by: kosuke55 * rename func Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../scene_module/lane_change/normal.hpp | 4 +- .../src/scene_module/lane_change/normal.cpp | 37 ++++++++++++++----- 2 files changed, 30 insertions(+), 11 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 8400cf8c40afd..54883489f2fe6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -158,10 +158,10 @@ class NormalLaneChange : public LaneChangeBase //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. //! @param obstacle_check_distance Distance to check ahead for any objects that might be //! obstructing ego path. It makes sense to use values like the maximum lane change distance. - bool isVehicleStuckByObstacle( + bool isVehicleStuck( const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const; - bool isVehicleStuckByObstacle(const lanelet::ConstLanelets & current_lanes) const; + bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const; double calcMaximumLaneChangeLength( const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 3a2903cb01e9e..ce07cda882692 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -83,7 +83,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) // find candidate paths LaneChangePaths valid_paths{}; - const bool is_stuck = isVehicleStuckByObstacle(current_lanes); + const bool is_stuck = isVehicleStuck(current_lanes); bool found_safe_path = getLaneChangePaths( current_lanes, target_lanes, direction_, &valid_paths, lane_change_parameters_->rss_params, is_stuck); @@ -670,7 +670,7 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( } // If the ego is in stuck, sampling all possible accelerations to find avoiding path. - if (isVehicleStuckByObstacle(current_lanes)) { + if (isVehicleStuck(current_lanes)) { auto clock = rclcpp::Clock(RCL_ROS_TIME); RCLCPP_INFO_THROTTLE( logger_, clock, 1000, "Vehicle is in stuck. Sample all possible acc: [%f ~ %f]", min_acc, @@ -1333,7 +1333,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const debug_filtered_objects_ = target_objects; CollisionCheckDebugMap debug_data; - const bool is_stuck = isVehicleStuckByObstacle(current_lanes); + const bool is_stuck = isVehicleStuck(current_lanes); const auto safety_status = isLaneChangePathSafe( path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data); { @@ -1674,8 +1674,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return path_safety_status; } -// Check if the ego vehicle is in stuck by a stationary obstacle. -bool NormalLaneChange::isVehicleStuckByObstacle( +// Check if the ego vehicle is in stuck by a stationary obstacle or by the terminal of current lanes +bool NormalLaneChange::isVehicleStuck( const lanelet::ConstLanelets & current_lanes, const double obstacle_check_distance) const { // Ego is still moving, not in stuck @@ -1709,12 +1709,31 @@ bool NormalLaneChange::isVehicleStuckByObstacle( } } - // No stationary objects found in obstacle_check_distance - RCLCPP_DEBUG(logger_, "No stationary objects found in obstacle_check_distance"); + // Check if Ego is in terminal of current lanes + const auto & route_handler = getRouteHandler(); + const double distance_to_terminal = + route_handler->isInGoalRouteSection(current_lanes.back()) + ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) + : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); + const double lane_change_buffer = + utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; + if (distance_to_terminal < terminal_judge_buffer) { + return true; + } + + // No stationary objects found in obstacle_check_distance and Ego is not in terminal of current + RCLCPP_DEBUG( + logger_, + "No stationary objects found in obstacle_check_distance and Ego is not in " + "terminal of current lanes"); return false; } -bool NormalLaneChange::isVehicleStuckByObstacle(const lanelet::ConstLanelets & current_lanes) const +bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const { if (current_lanes.empty()) { return false; // can not check @@ -1736,7 +1755,7 @@ bool NormalLaneChange::isVehicleStuckByObstacle(const lanelet::ConstLanelets & c getCommonParam().base_link2front + DETECTION_DISTANCE_MARGIN; RCLCPP_DEBUG(logger_, "max_lane_change_length: %f, max_acc: %f", max_lane_change_length, max_acc); - return isVehicleStuckByObstacle(current_lanes, detection_distance); + return isVehicleStuck(current_lanes, detection_distance); } void NormalLaneChange::setStopPose(const Pose & stop_pose) From 16217e849b65047525c2c080cfca9ab77149f788 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 18 Oct 2023 10:27:17 +0900 Subject: [PATCH 117/206] fix(intersection): fix misuse of original path index to interpolated path (#5334) Signed-off-by: Mamoru Sobue --- .../src/util.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 08c818c2e49ce..bd31f758f6d0d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1327,6 +1327,19 @@ TimeDistanceArray calcIntersectionPassingTime( dist_sum = 0.0; double passing_time = time_delay; time_distance_array.emplace_back(passing_time, dist_sum); + + // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so + // `last_intersection_stop_line_candidate_idx` makes no sense + const auto last_intersection_stop_line_candidate_point_orig = + path.points.at(last_intersection_stop_line_candidate_idx).point.pose; + const auto last_intersection_stop_line_candidate_nearest_ind_opt = motion_utils::findNearestIndex( + smoothed_reference_path.points, last_intersection_stop_line_candidate_point_orig, 3.0, M_PI_4); + if (!last_intersection_stop_line_candidate_nearest_ind_opt) { + return time_distance_array; + } + const auto last_intersection_stop_line_candidate_nearest_ind = + last_intersection_stop_line_candidate_nearest_ind_opt.value(); + for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) { const auto & p1 = smoothed_reference_path.points.at(i - 1); const auto & p2 = smoothed_reference_path.points.at(i); @@ -1338,7 +1351,7 @@ TimeDistanceArray calcIntersectionPassingTime( const double average_velocity = (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; const double minimum_ego_velocity_division = - (use_upstream_velocity && i > last_intersection_stop_line_candidate_idx) + (use_upstream_velocity && i > last_intersection_stop_line_candidate_nearest_ind) ? minimum_upstream_velocity /* to avoid null division */ : minimum_ego_velocity; const double passing_velocity = From f0ea398bd5fe71e51eb80116f0a29c035e2843a4 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 18 Oct 2023 10:27:43 +0900 Subject: [PATCH 118/206] fix(intersection): fix infinite loop in tsort (#5332) Signed-off-by: Mamoru Sobue --- .../src/util.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index bd31f758f6d0d..403509a926022 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -605,6 +605,8 @@ mergeLaneletsByTopologicalSort( id2lanelet[id] = lanelet; ind++; } + // NOTE: this function aims to traverse the detection lanelet backward from ego side to farthest + // side, so if lane B follows lane A on the routing_graph, adj[B][A] = true for (const auto & lanelet : lanelets) { const auto & followings = routing_graph_ptr->following(lanelet); const int dst = lanelet.id(); @@ -628,18 +630,25 @@ mergeLaneletsByTopologicalSort( if (!has_no_previous(src)) { continue; } + // So `src` has no previous lanelets branches[(ind2id[src])] = std::vector{}; auto & branch = branches[(ind2id[src])]; lanelet::Id node_iter = ind2id[src]; + std::set visited_ids; while (true) { const auto & destinations = adjacency[(id2ind[node_iter])]; - // NOTE: assuming detection lanelets have only one previous lanelet + // NOTE: assuming detection lanelets have only one "previous"(on the routing_graph) lanelet const auto next = std::find(destinations.begin(), destinations.end(), true); if (next == destinations.end()) { branch.push_back(node_iter); break; } + if (visited_ids.find(node_iter) != visited_ids.end()) { + // loop detected + break; + } branch.push_back(node_iter); + visited_ids.insert(node_iter); node_iter = ind2id[std::distance(destinations.begin(), next)]; } } From 22a5ced2c7a4d4b9421b508cf90e4d9018a073c9 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 18 Oct 2023 10:48:05 +0900 Subject: [PATCH 119/206] feat(behavior_path_planner): add postProcess() in scene module interface (#5336) * add postProcess Signed-off-by: kyoichi-sugahara * modify comment Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../include/behavior_path_planner/planner_manager.hpp | 2 ++ .../scene_module/scene_module_interface.hpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 72c32039da627..4b9b0bff4be63 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -272,6 +272,8 @@ class PlannerManager const auto result = module_ptr->run(); module_ptr->unlockRTCCommand(); + module_ptr->postProcess(); + module_ptr->updateCurrentState(); module_ptr->publishRTCStatus(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 077f7a3a763d5..68faa3806f614 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -131,6 +131,12 @@ class SceneModuleInterface */ virtual void updateData() {} + /** + * @brief After executing run(), update the module-specific status and/or data used for internal + * processing that are not defined in ModuleStatus. + */ + virtual void postProcess() {} + /** * @brief Execute module. Once this function is executed, * it will continue to run as long as it is in the RUNNING state. From a39b20d95200527726284101a0030dad08aa5a59 Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Wed, 18 Oct 2023 17:00:15 +0900 Subject: [PATCH 120/206] fix(detected_object_validation): consider shoulder lanelet in object lanelet filter (#5324) * fix(detected_object_validation): consider shoulder lanelet in object lanelet filter Signed-off-by: Shumpei Wakabayashi * style(pre-commit): autofix --------- Signed-off-by: Shumpei Wakabayashi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../detected_object_filter/object_lanelet_filter.hpp | 1 + .../src/object_lanelet_filter.cpp | 10 ++++++++-- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp index 600da31df5868..ae6162542a1c3 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp @@ -51,6 +51,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node lanelet::LaneletMapPtr lanelet_map_ptr_; lanelet::ConstLanelets road_lanelets_; + lanelet::ConstLanelets shoulder_lanelets_; std::string lanelet_frame_id_; tf2_ros::Buffer tf_buffer_; diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 3b78ae449e2da..f9b208cca02bc 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -62,6 +62,7 @@ void ObjectLaneletFilterNode::mapCallback( lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); } void ObjectLaneletFilterNode::objectCallback( @@ -87,7 +88,10 @@ void ObjectLaneletFilterNode::objectCallback( // calculate convex hull const auto convex_hull = getConvexHull(transformed_objects); // get intersected lanelets - lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull, road_lanelets_); + lanelet::ConstLanelets intersected_road_lanelets = + getIntersectedLanelets(convex_hull, road_lanelets_); + lanelet::ConstLanelets intersected_shoulder_lanelets = + getIntersectedLanelets(convex_hull, shoulder_lanelets_); int index = 0; for (const auto & object : transformed_objects.objects) { @@ -101,7 +105,9 @@ void ObjectLaneletFilterNode::objectCallback( polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); - if (isPolygonOverlapLanelets(polygon, intersected_lanelets)) { + if (isPolygonOverlapLanelets(polygon, intersected_road_lanelets)) { + output_object_msg.objects.emplace_back(input_msg->objects.at(index)); + } else if (isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets)) { output_object_msg.objects.emplace_back(input_msg->objects.at(index)); } } else { From 264accb7086239e6c128478bf0f548138db59068 Mon Sep 17 00:00:00 2001 From: melike tanrikulu <41450930+meliketanrikulu@users.noreply.github.com> Date: Wed, 18 Oct 2023 11:04:28 +0300 Subject: [PATCH 121/206] fix(gnss_poser): fix_transform_direction_problem (#5033) * fix(gnss_poser)fix_transform_direction_problem Signed-off-by: meliketanrikulu * style(pre-commit): autofix * fix(gnss_poser)update comment line Signed-off-by: meliketanrikulu --------- Signed-off-by: meliketanrikulu Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- sensing/gnss_poser/src/gnss_poser_core.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp index 599a017bffed7..3a18dca815f12 100644 --- a/sensing/gnss_poser/src/gnss_poser_core.cpp +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -139,11 +139,11 @@ void GNSSPoser::callbackNavSatFix( tf2::Transform tf_map2gnss_antenna{}; tf2::fromMsg(gnss_antenna_pose, tf_map2gnss_antenna); - // get TF from base_link to gnss_antenna + // get TF from gnss_antenna to base_link auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); getStaticTransform( - gnss_frame_, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + base_frame_, gnss_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); tf2::Transform tf_gnss_antenna2base_link{}; tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); From 5d175014c4972fead53a39ffb46d20fb8034f5e0 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Wed, 18 Oct 2023 17:22:45 +0900 Subject: [PATCH 122/206] feat(system_diagnostic_graph): change config file format (#5340) Signed-off-by: Takagi, Isamu --- system/system_diagnostic_graph/CMakeLists.txt | 8 +- system/system_diagnostic_graph/README.md | 38 ++- .../config/default.param.yaml | 2 + .../doc/overview.drawio.svg | 217 ++++++------- .../example/example.launch.xml | 4 +- .../example/example1.yaml | 41 --- .../example/example2.yaml | 32 -- .../example/example_0.yaml | 32 ++ .../example/example_1.yaml | 23 ++ .../example/example_2.yaml | 8 + .../example/example_diags.py | 64 ++++ .../example/publisher.py | 83 ----- .../src/core/config.cpp | 249 +++++++++++---- .../src/core/config.hpp | 90 ++++-- .../src/core/debug.cpp | 28 +- .../system_diagnostic_graph/src/core/expr.cpp | 284 ------------------ .../src/core/exprs.cpp | 216 +++++++++++++ .../src/core/{expr.hpp => exprs.hpp} | 46 ++- .../src/core/graph.cpp | 168 ++++++++--- .../src/core/graph.hpp | 27 +- .../src/core/modes.cpp | 75 +++++ .../src/core/{update.hpp => modes.hpp} | 45 ++- .../system_diagnostic_graph/src/core/node.cpp | 94 ------ .../system_diagnostic_graph/src/core/node.hpp | 84 ------ .../src/core/nodes.cpp | 157 ++++++++++ .../src/core/nodes.hpp | 114 +++++++ .../src/core/types.hpp | 7 +- .../src/core/update.cpp | 116 ------- system/system_diagnostic_graph/src/main.cpp | 35 ++- system/system_diagnostic_graph/src/main.hpp | 10 +- system/system_diagnostic_graph/src/tool.hpp | 1 - .../system_diagnostic_monitor/CMakeLists.txt | 7 - system/system_diagnostic_monitor/README.md | 1 - .../config/autoware.yaml | 74 ----- .../config/control.yaml | 10 - .../config/localization.yaml | 5 - .../system_diagnostic_monitor/config/map.yaml | 7 - .../config/perception.yaml | 7 - .../config/planning.yaml | 15 - .../config/system.yaml | 7 - .../config/vehicle.yaml | 10 - .../system_diagnostic_monitor.launch.xml | 6 - system/system_diagnostic_monitor/package.xml | 23 -- .../script/component_state_diagnostics.py | 79 ----- 44 files changed, 1299 insertions(+), 1350 deletions(-) delete mode 100644 system/system_diagnostic_graph/example/example1.yaml delete mode 100644 system/system_diagnostic_graph/example/example2.yaml create mode 100644 system/system_diagnostic_graph/example/example_0.yaml create mode 100644 system/system_diagnostic_graph/example/example_1.yaml create mode 100644 system/system_diagnostic_graph/example/example_2.yaml create mode 100755 system/system_diagnostic_graph/example/example_diags.py delete mode 100755 system/system_diagnostic_graph/example/publisher.py delete mode 100644 system/system_diagnostic_graph/src/core/expr.cpp create mode 100644 system/system_diagnostic_graph/src/core/exprs.cpp rename system/system_diagnostic_graph/src/core/{expr.hpp => exprs.hpp} (68%) create mode 100644 system/system_diagnostic_graph/src/core/modes.cpp rename system/system_diagnostic_graph/src/core/{update.hpp => modes.hpp} (53%) delete mode 100644 system/system_diagnostic_graph/src/core/node.cpp delete mode 100644 system/system_diagnostic_graph/src/core/node.hpp create mode 100644 system/system_diagnostic_graph/src/core/nodes.cpp create mode 100644 system/system_diagnostic_graph/src/core/nodes.hpp delete mode 100644 system/system_diagnostic_graph/src/core/update.cpp delete mode 100644 system/system_diagnostic_monitor/CMakeLists.txt delete mode 100644 system/system_diagnostic_monitor/README.md delete mode 100644 system/system_diagnostic_monitor/config/autoware.yaml delete mode 100644 system/system_diagnostic_monitor/config/control.yaml delete mode 100644 system/system_diagnostic_monitor/config/localization.yaml delete mode 100644 system/system_diagnostic_monitor/config/map.yaml delete mode 100644 system/system_diagnostic_monitor/config/perception.yaml delete mode 100644 system/system_diagnostic_monitor/config/planning.yaml delete mode 100644 system/system_diagnostic_monitor/config/system.yaml delete mode 100644 system/system_diagnostic_monitor/config/vehicle.yaml delete mode 100644 system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml delete mode 100644 system/system_diagnostic_monitor/package.xml delete mode 100755 system/system_diagnostic_monitor/script/component_state_diagnostics.py diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 54aebe6f37f73..81d8ba39b3d1b 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -5,13 +5,13 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_executable(aggregator - src/main.cpp src/core/config.cpp src/core/debug.cpp src/core/graph.cpp - src/core/node.cpp - src/core/expr.cpp - src/core/update.cpp + src/core/nodes.cpp + src/core/exprs.cpp + src/core/modes.cpp + src/main.cpp ) ament_auto_add_executable(converter diff --git a/system/system_diagnostic_graph/README.md b/system/system_diagnostic_graph/README.md index c2c99938348e2..df22121a778fb 100644 --- a/system/system_diagnostic_graph/README.md +++ b/system/system_diagnostic_graph/README.md @@ -2,28 +2,42 @@ ## Overview -The system diagnostic graph node subscribes to diagnostic status and publishes aggregated diagnostic status. +The system diagnostic graph node subscribes to diagnostic array and publishes aggregated diagnostic graph. As shown in the diagram below, this node introduces extra diagnostic status for intermediate functional unit. Diagnostic status dependencies will be directed acyclic graph (DAG). ![overview](./doc/overview.drawio.svg) +## Operation mode availability + +For MRM, this node publishes the status of the top-level functional units in the dedicated message. +Therefore, the diagnostic graph must contain functional units with the following names. +This feature breaks the generality of the graph and may be changed to a plugin or another node in the future. + +- /autoware/operation/stop +- /autoware/operation/autonomous +- /autoware/operation/local +- /autoware/operation/remote +- /autoware/operation/emergency-stop +- /autoware/operation/comfortable-stop +- /autoware/operation/pull-over + ## Interface -| Interface Type | Interface Name | Data Type | Description | -| -------------- | --------------------------- | --------------------------------------- | ------------------ | -| subscription | `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Input diagnostics. | -| publisher | `/diagnostics_graph/status` | `diagnostic_msgs/msg/DiagnosticArray` | Graph status. | -| publisher | `/diagnostics_graph/struct` | `tier4_system_msgs/msg/DiagnosticGraph` | Graph structure. | +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------------- | ------------------------------------------------- | ------------------ | +| subscription | `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Diagnostics input. | +| publisher | `/diagnostics_graph` | `tier4_system_msgs/msg/DiagnosticGraph` | Diagnostics graph. | +| publisher | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | mode availability. | ## Parameters -| Parameter Name | Data Type | Description | -| ------------------ | --------- | ------------------------------------------ | -| `graph_file` | `string` | Path of the config file. | -| `rate` | `double` | Rate of aggregation and topic publication. | -| `status_qos_depth` | `uint` | QoS depth of status topic. | -| `source_qos_depth` | `uint` | QoS depth of source topic. | +| Parameter Name | Data Type | Description | +| ----------------- | --------- | ------------------------------------------ | +| `graph_file` | `string` | Path of the config file. | +| `rate` | `double` | Rate of aggregation and topic publication. | +| `input_qos_depth` | `uint` | QoS depth of input array topic. | +| `graph_qos_depth` | `uint` | QoS depth of output graph topic. | ## Graph file format diff --git a/system/system_diagnostic_graph/config/default.param.yaml b/system/system_diagnostic_graph/config/default.param.yaml index f02247374b14e..9fd572c7926fa 100644 --- a/system/system_diagnostic_graph/config/default.param.yaml +++ b/system/system_diagnostic_graph/config/default.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + mode_availability: true + mode: psim rate: 1.0 input_qos_depth: 1000 graph_qos_depth: 1 diff --git a/system/system_diagnostic_graph/doc/overview.drawio.svg b/system/system_diagnostic_graph/doc/overview.drawio.svg index aa6be5a48b08e..e971c052dedc8 100644 --- a/system/system_diagnostic_graph/doc/overview.drawio.svg +++ b/system/system_diagnostic_graph/doc/overview.drawio.svg @@ -4,13 +4,13 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="921px" - height="561px" - viewBox="-0.5 -0.5 921 561" - content="<mxfile><diagram id="pC5EcGJeU-t8W7RoQB87" name="Page-1">3VtNc6M4EP01vpsPYfvoOJ7Z2sruTiVbtccpBRTQDtAuIcfx/vqVQBiQcGKXGdDkZGgakF4/tfpJeOZtsrevDO+SPyAi6cydR28z737mug5yFuJHWo6VBS1RZYgZjZRTY3ii/xFlnCvrnkak6DhygJTTXdcYQp6TkHdsmDE4dN1eIO2+dYdjYhieQpya1n9oxJPKunQXjf03QuOkfrMTrKorGa6dVU+KBEdwaJm87czbMABeHWVvG5JK8Gpcqvu+nLl6ahgjOb/kBtev7njF6V51TjhHFMc5FJyGxfdYhvB7wTHfF6rN/FgDwWCfR0Q+az7z7g4J5eRph0N59SDuE7aEZ6k4c8QhTmmci+OUvIi23b0SJl6A07Uyc5D+hbid5vFD6XOPhCXCRVK+Qj7D7J/qsnwaeWuZVH+/EsgIZ0fhoq76Cvpj9/TQBHJVRydpBREFyogVeeLTkxt8xYGC+Azc6AO4PwXCSw3hHoiXfg/EjjsExIGBIYnE6FWnwHgCMeQ43TbWuy7Kjc8DSMhKXP4lnB9VKsJ7Dl3kz6JWwJ6Fqh11fuCYxUS5eYoRso3vYstIijl97WagPqDKW9eM4WPLYQc050Xryd+koQmZp48KT8sb1/mLg6oFTchOXbksigtjoPwtQuHOH+j9+vG2UTIAww2KL02KO31ZxB+C4UtrGa5Y0GW4ZwXDkaPn/fcZHgTv+t/O8NUvFUW0tCKK/uq6KH7gf3MUa6haeeoLg7IFdmQqX0/UY2YqzzH6bw3HvZ5MtbCC4wHqhqyebC6dizX/2znu/lpRXNkRxcV1UfzA//YoemczFTwLeReKsAlFTrgQzBTyyRNXMO/i4bljJi7fXsojaxOXkYiCKxNXMDDlTbX9DQpJcyLEdoat4LkuJcblub1i2esRy2hwKXExUKYeXYt+5ZCBXBabZxAR66jkzsekksWqdGUVlVYGlTaQvYjO4+dyDi7kOt3UXNKn31G5VD/DQi759X6DFVyqW9Pi0jYjLM7Doy1EOpFkEiLZK118ewWoo88jEwtQ35Quj7Dn00+4OrdHXVzx7dUofo9GmTBJmjLgdzjK7bYftlRui2DKJGmvCPB7RIA/+I7ZxUCZIqAhkmUMGjcVLQ1g1n/eG5iIrvBuxwvO4AfZQApMWHLIJbVeaJpqpnpjOxQAEdaztZ3RKCp52Yd0NxYDgG1MeMgEu2+De4j9bX+SfaPeQJ1FcoSa5sMixHE0sKu8ou5q8L51kcvxryuONP+bi6Oaeu11XZrKUeLOn2XH9uWKV4jLH5xHkjsiaZGsJBHuKaM+zThFmoh1PHOcnj4qG3qgIns3v5BrjtPp6rO6NS0GP5XK1Y7aTN9BHbU2Q6b02TIGrIzDTvBjcnh0DdRTd5xqk8HRsVcCob5tGju+hNG/bPEW130Jo/nfPoOZ+kztTAY4k4TNn4tyesARZpPzXV8YHbXQRoEB1V+Pn3f+Pse8MepstLA3tyx7cosdS4d6xTX10iEylWnPVw87RiJqx2cPerXxM7eDZxKL+p8GFbjN/zW87f8=</diagram></mxfile>" + height="521px" + viewBox="-0.5 -0.5 921 521" + content="<mxfile><diagram id="pC5EcGJeU-t8W7RoQB87" name="Page-1">3VpNb6MwEP01ufMdckyTtntod1ftSnusXHDBrWEi43ywv37tYELAtE0UCqiXyDzGxn7zPB6PMrEXye6WoVV8DyGmE8sIdxN7ObEs3zXErwTyAvBMtwAiRsICMivgkfzDClT9ojUJcVYz5ACUk1UdDCBNccBrGGIMtnWzF6D1r65QhDXgMUBUR/+SkMdqWda0wn9gEsXll01vVrxJUGmsVpLFKITtEWRfT+wFA+BFK9ktMJXclbwU/W7eeXuYGMMpP6WD5RQ9Noiu1eKEcUhQlELGSZA9RdKDarI8LxlgsE5DLAcxJvbVNiYcP65QIN9uRQeBxTyh4skUTURJlIo2xS9iUlcbzMTIiM4VzEHaZ6I7SaO7vc3SFUiIsnj/CTmGvjC1Vjka3h1BaqG3GBLMWS5M1FtHcZ7XH7eVB2elW+Ij77kliJRqosPIFbGiobht59mdtfGc5RnHiWjACjPECaRPidgrAkAbRCh6JpTw/FuQ79fJ93XyfaeFfLML8i33E5F/R4Ydo1eKPY1DHIqYqR6B8RgiSBG9rtCrOsuVzR1Iyva8vGLOc3UAoDWHOvPvspbBmgVqHmVU5ohFWJnZShFyjh9yyzAV+3JTj/ttRO27zhlD+ZHBCkjKs6ORf0ugcplr1H1m+41ofZ69aBQzqFx2WMppXpxqG+WPcIVl3JHl/OGyXdJFAG9G8JYQbraFcKcLhfujVbhttCjcHoXCvWlDsd7HCp86H9pfrPCSqiOF3zDYz2AcGveMATVum9r6R6Nxu0Xj01FovKnZ0hWnRvGG/eUatzWNw3PGUSDcJe5cmIsrkUg1h5d6I2NpSwq/TurOeKXujlbqmnSNM6VudCx1PbNfQSZljkVin6BR6LyZtvSrcz2lk7JMIYH1hfeeL6DmQEMf1DimRg3DCXA8OC3Nu1yvtHjWxZHx3IjntBzupXe6i3gn60I/P0tdGAEkCUrD0Smk1zTR0aMuhQDRwVk5rHiQcDLeEojTUgJxOi+BnEyUfiS9Qi4LcW8j2mFNLfW7w3yNovnPpcaJWAqvLzzjDN7wAigwgaSQSpG9EEobUFmzDARBmLVULRMShnuFtjFd90UHZGt5oqmT3Va77KJ06cyG2LetjnqXyR6uAp/m7tMG10WAUZ0qui+9UphnXinMjq8UrqVtvUyW8IeOR83CUK9nmzve27LbdlseR/HTbxwhQxc/XT1vU8VPDyVSselztg/vKERscMFPvQEPYNfTqPr18H3PX015dn/nr6dTff9wPzr5fWW8nch9WP7lo9jY1f9m7Ov/</diagram></mxfile>" > - + @@ -20,243 +20,231 @@ >
- /diagnostics_graph_status + /diagnostics_graph
- /diagnostics_graph_status + /diagnostics_graph - +
- /diagnostics + /system/operation_mode/availability
- /diagnostics + /system/operation_mode/availability - - - +
-
+
- Top LiDAR + /diagnostics
- Top LiDAR + /diagnostics - - - - - + + +
- Front LiDAR + Top LiDAR
- Front LiDAR + Top LiDAR - - - - - + + +
- Front obstacle detection + Front LiDAR
- Front obstacle detec... + Front LiDAR - - - + + +
- Pose estimation + obstacle detection
- Pose estimation + obstacle detection - - - + + +
- Autonomous mode + pose estimation
- Autonomous mode + pose estimation - - - +
- Comfortable stop + autonomous
- Comfortable stop + autonomous - - - +
- Emergncy stop + remote
- Emergncy stop + remote - - - + + +
- Route + remote command
- Route + remote command - - - +
- Joystick mode + local
- Joystick mode + local - - - + + +
- Joystick + joystick command
- Joystick + joystick command - +
@@ -265,73 +253,38 @@
- AND + AND - - - + + +
- Filter by use case and system state -
-
-
- - Filter by use case and system state - - - - - - - - -
-
-
- Stop mode -
-
-
- - Stop mode - - - - - - -
-
-
- Error report + stop
- Error report + stop - - - + + +
@@ -340,16 +293,16 @@
- Front radar + Front radar - +
@@ -358,27 +311,25 @@
- OR + OR - - - +
- Front obstacle prediction + MRM
- Front obstacle predi... + MRM diff --git a/system/system_diagnostic_graph/example/example.launch.xml b/system/system_diagnostic_graph/example/example.launch.xml index 461842f020ded..1456bfd5c6593 100644 --- a/system/system_diagnostic_graph/example/example.launch.xml +++ b/system/system_diagnostic_graph/example/example.launch.xml @@ -1,6 +1,6 @@ - + - + diff --git a/system/system_diagnostic_graph/example/example1.yaml b/system/system_diagnostic_graph/example/example1.yaml deleted file mode 100644 index 25053b5f7e12c..0000000000000 --- a/system/system_diagnostic_graph/example/example1.yaml +++ /dev/null @@ -1,41 +0,0 @@ -files: - - { package: system_diagnostic_graph, path: example/example2.yaml } - -nodes: - - name: /autoware/diagnostics - type: and - list: - - { type: unit, name: /autoware/operation/stop } - - { type: unit, name: /autoware/operation/autonomous } - - { type: unit, name: /autoware/operation/local } - - { type: unit, name: /autoware/operation/remote } - - { type: unit, name: /autoware/operation/emergency-stop } - - { type: unit, name: /autoware/operation/comfortable-stop } - - - name: /autoware/operation/stop - type: debug-ok - - - name: /autoware/operation/autonomous - type: and - list: - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - - name: /autoware/operation/local - type: debug-ok - - - name: /autoware/operation/remote - type: and - list: - - { type: diag, hardware: test, name: /external/remote_command } - - - name: /autoware/operation/emergency-stop - type: debug-ok - - - name: /autoware/operation/comfortable-stop - type: and - list: - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception/front-obstacle-detection } diff --git a/system/system_diagnostic_graph/example/example2.yaml b/system/system_diagnostic_graph/example/example2.yaml deleted file mode 100644 index 2af09c0cb69a2..0000000000000 --- a/system/system_diagnostic_graph/example/example2.yaml +++ /dev/null @@ -1,32 +0,0 @@ -nodes: - - name: /autoware/localization - type: and - list: - - { type: diag, hardware: test, name: /sensing/lidars/top } - - - name: /autoware/planning - type: and - list: - - { type: unit, name: /autoware/planning/route } - - - name: /autoware/planning/route - type: and - list: - - { type: diag, hardware: test, name: /planning/route } - - - name: /autoware/perception - type: and - list: - - { type: unit, name: /autoware/perception/front-obstacle-detection } - - { type: unit, name: /autoware/perception/front-obstacle-prediction } - - - name: /autoware/perception/front-obstacle-detection - type: or - list: - - { type: diag, hardware: test, name: /sensing/lidars/front } - - { type: diag, hardware: test, name: /sensing/radars/front } - - - name: /autoware/perception/front-obstacle-prediction - type: and - list: - - { type: diag, hardware: test, name: /sensing/lidars/front } diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml new file mode 100644 index 0000000000000..0ee6e59c8e121 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -0,0 +1,32 @@ +files: + - { package: system_diagnostic_graph, path: example/example_1.yaml } + - { package: system_diagnostic_graph, path: example/example_2.yaml } + +nodes: + - path: /autoware/modes/stop + type: debug-ok + + - path: /autoware/modes/autonomous + type: and + list: + - { type: link, path: /functions/pose_estimation } + - { type: link, path: /functions/obstacle_detection } + + - path: /autoware/modes/local + type: and + list: + - { type: link, path: /external/joystick_command } + + - path: /autoware/modes/remote + type: and + list: + - { type: link, path: /external/remote_command } + + - path: /autoware/modes/emergency-stop + type: debug-ok + + - path: /autoware/modes/comfortable-stop + type: debug-ok + + - path: /autoware/modes/pull-over + type: debug-ok diff --git a/system/system_diagnostic_graph/example/example_1.yaml b/system/system_diagnostic_graph/example/example_1.yaml new file mode 100644 index 0000000000000..5ba93ec3059e4 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_1.yaml @@ -0,0 +1,23 @@ +nodes: + - path: /functions/pose_estimation + type: and + list: + - { type: link, path: /sensing/lidars/top } + + - path: /functions/obstacle_detection + type: or + list: + - { type: link, path: /sensing/lidars/front } + - { type: link, path: /sensing/radars/front } + + - path: /sensing/lidars/top + type: diag + name: "lidar_driver/top: status" + + - path: /sensing/lidars/front + type: diag + name: "lidar_driver/front: status" + + - path: /sensing/radars/front + type: diag + name: "radar_driver/front: status" diff --git a/system/system_diagnostic_graph/example/example_2.yaml b/system/system_diagnostic_graph/example/example_2.yaml new file mode 100644 index 0000000000000..f61f4accbfec8 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_2.yaml @@ -0,0 +1,8 @@ +nodes: + - path: /external/joystick_command + type: diag + name: "external_command_checker: joystick_command" + + - path: /external/remote_command + type: diag + name: "external_command_checker: remote_command" diff --git a/system/system_diagnostic_graph/example/example_diags.py b/system/system_diagnostic_graph/example/example_diags.py new file mode 100755 index 0000000000000..52d6189a63f30 --- /dev/null +++ b/system/system_diagnostic_graph/example/example_diags.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 + +# Copyright 2023 The Autoware Contributors +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from diagnostic_msgs.msg import DiagnosticArray +from diagnostic_msgs.msg import DiagnosticStatus +import rclpy +import rclpy.node +import rclpy.qos + + +class DummyDiagnostics(rclpy.node.Node): + def __init__(self, names): + super().__init__("dummy_diagnostics") + qos = rclpy.qos.qos_profile_system_default + self.diags = self.create_publisher(DiagnosticArray, "/diagnostics", qos) + self.timer = self.create_timer(0.5, self.on_timer) + self.count = 0 + self.array = [self.create_status(name) for name in names] + + def on_timer(self): + error_index = int(self.count / 10) + for index, status in enumerate(self.array, 1): + if index == error_index: + status.level = DiagnosticStatus.ERROR + status.message = "ERROR" + else: + status.level = DiagnosticStatus.OK + status.message = "OK" + + diagnostics = DiagnosticArray() + diagnostics.header.stamp = self.get_clock().now().to_msg() + diagnostics.status = self.array + self.count = (self.count + 1) % 60 + self.diags.publish(diagnostics) + + @staticmethod + def create_status(name: str): + return DiagnosticStatus(name=name, hardware_id="example") + + +if __name__ == "__main__": + diags = [ + "lidar_driver/top: status", + "lidar_driver/front: status", + "radar_driver/front: status", + "external_command_checker: joystick_command", + "external_command_checker: remote_command", + ] + rclpy.init() + rclpy.spin(DummyDiagnostics(diags)) + rclpy.shutdown() diff --git a/system/system_diagnostic_graph/example/publisher.py b/system/system_diagnostic_graph/example/publisher.py deleted file mode 100755 index 2e73237284508..0000000000000 --- a/system/system_diagnostic_graph/example/publisher.py +++ /dev/null @@ -1,83 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 The Autoware Contributors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import argparse - -from diagnostic_msgs.msg import DiagnosticArray -from diagnostic_msgs.msg import DiagnosticStatus -import rclpy -import rclpy.node -import rclpy.qos - - -class DummyDiagnostics(rclpy.node.Node): - def __init__(self, array): - super().__init__("dummy_diagnostics") - qos = rclpy.qos.qos_profile_system_default - self.diags = self.create_publisher(DiagnosticArray, "/diagnostics", qos) - self.timer = self.create_timer(0.5, self.on_timer) - self.array = [self.create_status(*data) for data in array] - - def on_timer(self): - diagnostics = DiagnosticArray() - diagnostics.header.stamp = self.get_clock().now().to_msg() - diagnostics.status = self.array - self.diags.publish(diagnostics) - - @staticmethod - def create_status(name: str, level: int): - return DiagnosticStatus(level=level, name=name, message="OK", hardware_id="test") - - -if __name__ == "__main__": - data = { - "ok": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.OK), - ("/sensing/radars/front", DiagnosticStatus.OK), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - "front-lidar": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.ERROR), - ("/sensing/radars/front", DiagnosticStatus.OK), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - "front-radar": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.OK), - ("/sensing/radars/front", DiagnosticStatus.ERROR), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - "front": [ - ("/sensing/lidars/top", DiagnosticStatus.OK), - ("/sensing/lidars/front", DiagnosticStatus.ERROR), - ("/sensing/radars/front", DiagnosticStatus.ERROR), - ("/planning/route", DiagnosticStatus.OK), - ("/external/remote_command", DiagnosticStatus.OK), - ], - } - - parser = argparse.ArgumentParser() - parser.add_argument("--data", default="ok") - args = parser.parse_args() - - rclpy.init() - rclpy.spin(DummyDiagnostics(data[args.data])) - rclpy.shutdown() diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index b33a7deb66fb6..305860af32840 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -17,106 +17,245 @@ #include #include -#include #include #include +#include + +// DEBUG +#include namespace system_diagnostic_graph { -ConfigError create_error(const FileConfig & config, const std::string & message) +ErrorMarker::ErrorMarker(const std::string & file) +{ + file_ = file; +} + +std::string ErrorMarker::str() const +{ + if (type_.empty()) { + return file_; + } + + std::string result = type_; + for (const auto & index : indices_) { + result += "-" + std::to_string(index); + } + return result + " in " + file_; +} + +ErrorMarker ErrorMarker::type(const std::string & type) const { - const std::string marker = config ? "File:" + config->path : "Parameter"; - return ConfigError(message + " (" + marker + ")"); + ErrorMarker mark = *this; + mark.type_ = type; + return mark; } -ConfigError create_error(const NodeConfig & config, const std::string & message) +ErrorMarker ErrorMarker::index(size_t index) const { - const std::string marker = "File:" + config->path + ", Node:" + config->name; - return ConfigError(message + " (" + marker + ")"); + ErrorMarker mark = *this; + mark.indices_.push_back(index); + return mark; } -NodeConfig parse_config_node(YAML::Node yaml, const FileConfig & scope) +ConfigObject::ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type) { if (!yaml.IsMap()) { - throw create_error(scope, "node object is not a dict"); + throw create_error(mark, type + " is not a dict type"); } - if (!yaml["name"]) { - throw create_error(scope, "node object has no 'name' field"); + for (const auto & kv : yaml) { + dict_[kv.first.as()] = kv.second; } + mark_ = mark; + type_ = type; +} - const auto config = std::make_shared(); - config->path = scope->path; - config->name = take(yaml, "name"); - config->yaml = yaml; - return config; +ErrorMarker ConfigObject::mark() const +{ + return mark_; } -FileConfig parse_config_path(YAML::Node yaml, const FileConfig & scope) +std::optional ConfigObject::take_yaml(const std::string & name) { - if (!yaml.IsMap()) { - throw create_error(scope, "file object is not a dict"); + if (!dict_.count(name)) { + return std::nullopt; } - if (!yaml["package"]) { - throw create_error(scope, "file object has no 'package' field"); + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml; +} + +std::string ConfigObject::take_text(const std::string & name) +{ + if (!dict_.count(name)) { + throw create_error(mark_, "object has no '" + name + "' field"); } - if (!yaml["path"]) { - throw create_error(scope, "file object has no 'path' field"); + + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml.as(); +} + +std::string ConfigObject::take_text(const std::string & name, const std::string & fail) +{ + if (!dict_.count(name)) { + return fail; } - const auto package_name = yaml["package"].as(); - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return parse_config_path(package_path + "/" + yaml["path"].as(), scope); + const auto yaml = dict_.at(name); + dict_.erase(name); + return yaml.as(); } -FileConfig parse_config_path(const std::string & path, const FileConfig & scope) +std::vector ConfigObject::take_list(const std::string & name) { - if (!std::filesystem::exists(path)) { - throw create_error(scope, "config file '" + path + "' does not exist"); + if (!dict_.count(name)) { + return std::vector(); } - return parse_config_file(path); + + const auto yaml = dict_.at(name); + dict_.erase(name); + + if (!yaml.IsSequence()) { + throw ConfigError("the '" + name + "' field is not a list type"); + } + return std::vector(yaml.begin(), yaml.end()); +} + +bool ConfigFilter::check(const std::string & mode) const +{ + if (!excludes.empty() && excludes.count(mode) != 0) return false; + if (!includes.empty() && includes.count(mode) == 0) return false; + return true; } -FileConfig parse_config_file(const std::string & path) +ConfigError create_error(const ErrorMarker & mark, const std::string & message) { - const auto config = std::make_shared(); - config->path = path; + (void)mark; + return ConfigError(message); +} - const auto yaml = YAML::LoadFile(path); - if (!yaml.IsMap()) { - throw create_error(config, "config file is not a dict"); +ConfigFilter parse_mode_filter(const ErrorMarker & mark, std::optional yaml) +{ + std::unordered_set excludes; + std::unordered_set includes; + if (yaml) { + ConfigObject dict(mark, yaml.value(), "mode filter"); + + for (const auto & mode : dict.take_list("except")) { + excludes.emplace(mode.as()); + } + for (const auto & mode : dict.take_list("only")) { + includes.emplace(mode.as()); + } } + return ConfigFilter{excludes, includes}; +} + +FileConfig parse_file_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "file object"); + const auto relative_path = dict.take_text("path"); + const auto package_name = dict.take_text("package"); + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return FileConfig{mark, package_path + "/" + relative_path}; +} + +NodeConfig parse_node_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "node object"); + const auto path = dict.take_text("path"); + const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); + return NodeConfig{path, mode, dict}; +} + +ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml) +{ + ConfigObject dict(mark, yaml, "expr object"); + return parse_expr_config(dict); +} + +ExprConfig parse_expr_config(ConfigObject & dict) +{ + const auto type = dict.take_text("type"); + const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); + return ExprConfig{type, mode, dict}; +} - const auto files = yaml["files"] ? yaml["files"] : YAML::Node(YAML::NodeType::Sequence); - if (!files.IsSequence()) { - throw create_error(config, "files section is not a list"); +void dump(const ConfigFile & config) +{ + std::cout << "=================================================================" << std::endl; + std::cout << config.mark.str() << std::endl; + for (const auto & file : config.files) { + std::cout << " - f: " << file.path << " (" << file.mark.str() << ")" << std::endl; + } + for (const auto & unit : config.units) { + std::cout << " - u: " << unit.path << " (" << unit.dict.mark().str() << ")" << std::endl; } - for (const auto file : files) { - config->files.push_back(parse_config_path(file, config)); + for (const auto & diag : config.diags) { + std::cout << " - d: " << diag.path << " (" << diag.dict.mark().str() << ")" << std::endl; } +} - const auto nodes = yaml["nodes"] ? yaml["nodes"] : YAML::Node(YAML::NodeType::Sequence); - if (!nodes.IsSequence()) { - throw create_error(config, "nodes section is not a list"); +template +auto apply(const ErrorMarker & mark, F & func, const std::vector & list) +{ + std::vector result; + for (size_t i = 0; i < list.size(); ++i) { + result.push_back(func(mark.index(i), list[i])); } - for (const auto node : nodes) { - config->nodes.push_back(parse_config_node(node, config)); + return result; +} + +ConfigFile load_config_file(const FileConfig & file) +{ + if (!std::filesystem::exists(file.path)) { + throw create_error(file.mark, "config file '" + file.path + "' does not exist"); } + const auto yaml = YAML::LoadFile(file.path); + const auto mark = ErrorMarker(file.path); + auto dict = ConfigObject(mark, yaml, "config file"); + + std::vector units; + std::vector diags; + for (const auto & node : dict.take_list("nodes")) { + const auto type = node["type"].as(); + if (type == "diag") { + diags.push_back(node); + } else { + units.push_back(node); + } + } + + ConfigFile config(mark); + config.files = apply(mark.type("file"), parse_file_config, dict.take_list("files")); + config.units = apply(mark.type("unit"), parse_node_config, units); + config.diags = apply(mark.type("diag"), parse_node_config, diags); return config; } -void walk_config_tree(const FileConfig & config, std::vector & nodes) +ConfigFile load_config_root(const std::string & path) { - nodes.insert(nodes.end(), config->nodes.begin(), config->nodes.end()); - for (const auto & file : config->files) walk_config_tree(file, nodes); -} + const auto mark = ErrorMarker("root file"); + std::vector configs; + configs.push_back(load_config_file(FileConfig{mark, path})); -std::vector load_config_file(const std::string & path) -{ - std::vector nodes; - walk_config_tree(parse_config_path(path, nullptr), nodes); - return nodes; + // Use an index because updating the vector invalidates the iterator. + for (size_t i = 0; i < configs.size(); ++i) { + for (const auto & file : configs[i].files) { + configs.push_back(load_config_file(file)); + } + } + + ConfigFile result(mark); + for (const auto & config : configs) { + result.files.insert(result.files.end(), config.files.begin(), config.files.end()); + result.units.insert(result.units.end(), config.units.begin(), config.units.end()); + result.diags.insert(result.diags.end(), config.diags.begin(), config.diags.end()); + } + return result; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index 6393cb906b119..4d679ef575944 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -18,8 +18,11 @@ #include #include +#include #include #include +#include +#include #include namespace system_diagnostic_graph @@ -30,38 +33,79 @@ struct ConfigError : public std::runtime_error using runtime_error::runtime_error; }; -struct NodeConfig_ +class ErrorMarker +{ +public: + explicit ErrorMarker(const std::string & file = ""); + std::string str() const; + ErrorMarker type(const std::string & type) const; + ErrorMarker index(size_t index) const; + +private: + std::string file_; + std::string type_; + std::vector indices_; +}; + +class ConfigObject +{ +public: + ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type); + ErrorMarker mark() const; + std::optional take_yaml(const std::string & name); + std::string take_text(const std::string & name); + std::string take_text(const std::string & name, const std::string & fail); + std::vector take_list(const std::string & name); + +private: + ErrorMarker mark_; + std::string type_; + std::unordered_map dict_; +}; + +struct ConfigFilter +{ + bool check(const std::string & mode) const; + std::unordered_set excludes; + std::unordered_set includes; +}; + +struct ExprConfig +{ + std::string type; + ConfigFilter mode; + ConfigObject dict; +}; + +struct NodeConfig { std::string path; - std::string name; - YAML::Node yaml; + ConfigFilter mode; + ConfigObject dict; }; -struct FileConfig_ +struct FileConfig { + ErrorMarker mark; std::string path; - std::vector> files; - std::vector> nodes; }; -template -T take(YAML::Node yaml, const std::string & field) +struct ConfigFile { - const auto result = yaml[field].as(); - yaml.remove(field); - return result; -} - -using NodeConfig = std::shared_ptr; -using FileConfig = std::shared_ptr; -ConfigError create_error(const FileConfig & config, const std::string & message); -ConfigError create_error(const NodeConfig & config, const std::string & message); -std::vector load_config_file(const std::string & path); - -NodeConfig parse_config_node(YAML::Node yaml, const FileConfig & scope); -FileConfig parse_config_path(YAML::Node yaml, const FileConfig & scope); -FileConfig parse_config_path(const std::string & path, const FileConfig & scope); -FileConfig parse_config_file(const std::string & path); + explicit ConfigFile(const ErrorMarker & mark) : mark(mark) {} + ErrorMarker mark; + std::vector files; + std::vector units; + std::vector diags; +}; + +using ConfigDict = std::unordered_map; + +ConfigError create_error(const ErrorMarker & mark, const std::string & message); +ConfigFile load_config_root(const std::string & path); + +ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml); +ExprConfig parse_expr_config(ConfigObject & dict); } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index 337be8c74aded..ed696573521a7 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -14,9 +14,9 @@ #include "debug.hpp" -#include "node.hpp" +#include "graph.hpp" +#include "nodes.hpp" #include "types.hpp" -#include "update.hpp" #include #include @@ -32,10 +32,10 @@ const std::unordered_map level_names = { {DiagnosticStatus::ERROR, "ERROR"}, {DiagnosticStatus::STALE, "STALE"}}; -void DiagGraph::debug() +void Graph::debug() { std::vector lines; - for (const auto & node : graph_.nodes()) { + for (const auto & node : nodes_) { lines.push_back(node->debug()); } @@ -59,17 +59,23 @@ void DiagGraph::debug() DiagDebugData UnitNode::debug() const { - const auto & level = node_.status.level; - const auto & name = node_.status.name; - return DiagDebugData{std::to_string(index()), "unit", name, "-----", level_names.at(level)}; + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"unit", index_name, level_name, path_, "-----"}; } DiagDebugData DiagNode::debug() const { - const auto & level = node_.status.level; - const auto & name = node_.status.name; - const auto & hardware = node_.status.hardware_id; - return DiagDebugData{std::to_string(index()), "diag", name, hardware, level_names.at(level)}; + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"diag", index_name, level_name, path_, name_}; +} + +DiagDebugData UnknownNode::debug() const +{ + const auto level_name = level_names.at(level()); + const auto index_name = std::to_string(index()); + return {"test", index_name, level_name, path_, "-----"}; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/expr.cpp b/system/system_diagnostic_graph/src/core/expr.cpp deleted file mode 100644 index dc7ebcf8dd859..0000000000000 --- a/system/system_diagnostic_graph/src/core/expr.cpp +++ /dev/null @@ -1,284 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "expr.hpp" - -#include "config.hpp" -#include "graph.hpp" -#include "node.hpp" - -#include -#include -#include -#include - -// -#include - -namespace system_diagnostic_graph -{ - -using LinkStatus = std::vector>; - -void extend(LinkStatus & a, const LinkStatus & b) -{ - a.insert(a.end(), b.begin(), b.end()); -} - -void extend_false(LinkStatus & a, const LinkStatus & b) -{ - for (const auto & p : b) { - a.push_back(std::make_pair(p.first, false)); - } -} - -std::unique_ptr BaseExpr::create(Graph & graph, YAML::Node yaml) -{ - if (!yaml.IsMap()) { - throw ConfigError("expr object is not a dict"); - } - if (!yaml["type"]) { - throw ConfigError("expr object has no 'type' field"); - } - - const auto type = take(yaml, "type"); - - if (type == "unit") { - return std::make_unique(graph, yaml); - } - if (type == "diag") { - return std::make_unique(graph, yaml); - } - if (type == "and") { - return std::make_unique(graph, yaml); - } - if (type == "or") { - return std::make_unique(graph, yaml); - } - if (type == "if") { - return std::make_unique(graph, yaml); - } - if (type == "debug-ok") { - return std::make_unique(DiagnosticStatus::OK); - } - if (type == "debug-warn") { - return std::make_unique(DiagnosticStatus::WARN); - } - if (type == "debug-error") { - return std::make_unique(DiagnosticStatus::ERROR); - } - if (type == "debug-stale") { - return std::make_unique(DiagnosticStatus::STALE); - } - throw ConfigError("unknown expr type: " + type); -} - -ConstExpr::ConstExpr(const DiagnosticLevel level) -{ - level_ = level; -} - -ExprStatus ConstExpr::eval() const -{ - ExprStatus status; - status.level = level_; - return status; -} - -std::vector ConstExpr::get_dependency() const -{ - return {}; -} - -UnitExpr::UnitExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["name"]) { - throw ConfigError("unit object has no 'name' field"); - } - const auto name = take(yaml, "name"); - node_ = graph.find_unit(name); - if (!node_) { - throw ConfigError("unit node '" + name + "' does not exist"); - } -} - -ExprStatus UnitExpr::eval() const -{ - ExprStatus status; - status.level = node_->level(); - status.links.push_back(std::make_pair(node_, true)); - return status; -} - -std::vector UnitExpr::get_dependency() const -{ - return {node_}; -} - -DiagExpr::DiagExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["name"]) { - throw ConfigError("diag object has no 'name' field"); - } - const auto name = yaml["name"].as(); - const auto hardware = yaml["hardware"].as(""); - node_ = graph.find_diag(name, hardware); - if (!node_) { - node_ = graph.make_diag(name, hardware); - } -} - -ExprStatus DiagExpr::eval() const -{ - ExprStatus status; - status.level = node_->level(); - status.links.push_back(std::make_pair(node_, true)); - return status; -} - -std::vector DiagExpr::get_dependency() const -{ - return {node_}; -} - -AndExpr::AndExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["list"]) { - throw ConfigError("expr object has no 'list' field"); - } - if (!yaml["list"].IsSequence()) { - throw ConfigError("list field is not a list"); - } - - for (const auto & node : yaml["list"]) { - list_.push_back(BaseExpr::create(graph, node)); - } -} - -ExprStatus AndExpr::eval() const -{ - std::vector results; - for (const auto & expr : list_) { - results.push_back(expr->eval()); - } - std::vector levels; - for (const auto & result : results) { - levels.push_back(result.level); - } - ExprStatus status; - for (const auto & result : results) { - extend(status.links, result.links); - } - const auto level = *std::max_element(levels.begin(), levels.end()); - status.level = std::min(level, DiagnosticStatus::ERROR); - return status; -} - -std::vector AndExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -OrExpr::OrExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["list"]) { - throw ConfigError("expr object has no 'list' field"); - } - if (!yaml["list"].IsSequence()) { - throw ConfigError("list field is not a list"); - } - - for (const auto & node : yaml["list"]) { - list_.push_back(BaseExpr::create(graph, node)); - } -} - -ExprStatus OrExpr::eval() const -{ - std::vector results; - for (const auto & expr : list_) { - results.push_back(expr->eval()); - } - std::vector levels; - for (const auto & result : results) { - levels.push_back(result.level); - } - ExprStatus status; - for (const auto & result : results) { - extend(status.links, result.links); - } - const auto level = *std::min_element(levels.begin(), levels.end()); - status.level = std::min(level, DiagnosticStatus::ERROR); - return status; -} - -std::vector OrExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -IfExpr::IfExpr(Graph & graph, YAML::Node yaml) -{ - if (!yaml["cond"]) { - throw ConfigError("expr object has no 'cond' field"); - } - if (!yaml["then"]) { - throw ConfigError("expr object has no 'then' field"); - } - cond_ = BaseExpr::create(graph, yaml["cond"]); - then_ = BaseExpr::create(graph, yaml["then"]); -} - -ExprStatus IfExpr::eval() const -{ - const auto cond = cond_->eval(); - const auto then = then_->eval(); - ExprStatus status; - if (cond.level == DiagnosticStatus::OK) { - status.level = std::min(then.level, DiagnosticStatus::ERROR); - extend(status.links, cond.links); - extend(status.links, then.links); - } else { - status.level = std::min(cond.level, DiagnosticStatus::ERROR); - extend(status.links, cond.links); - extend_false(status.links, then.links); - } - return status; -} - -std::vector IfExpr::get_dependency() const -{ - std::vector depends; - { - const auto nodes = cond_->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - { - const auto nodes = then_->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/exprs.cpp b/system/system_diagnostic_graph/src/core/exprs.cpp new file mode 100644 index 0000000000000..22281f939cad2 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/exprs.cpp @@ -0,0 +1,216 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "exprs.hpp" + +#include "nodes.hpp" + +#include +#include +#include +#include +#include + +// DEBUG +#include + +namespace system_diagnostic_graph +{ + +using LinkStatus = std::vector>; + +void extend(LinkStatus & a, const LinkStatus & b) +{ + a.insert(a.end(), b.begin(), b.end()); +} + +void extend_false(LinkStatus & a, const LinkStatus & b) +{ + for (const auto & p : b) { + a.push_back(std::make_pair(p.first, false)); + } +} + +auto create_expr_list(ExprInit & exprs, ConfigObject & config) +{ + std::vector> result; + const auto list = config.take_list("list"); + for (size_t i = 0; i < list.size(); ++i) { + auto dict = parse_expr_config(config.mark().index(i), list[i]); + auto expr = exprs.create(dict); + if (expr) { + result.push_back(std::move(expr)); + } + } + return result; +} + +ConstExpr::ConstExpr(const DiagnosticLevel level) +{ + level_ = level; +} + +ExprStatus ConstExpr::eval() const +{ + ExprStatus status; + status.level = level_; + return status; +} + +std::vector ConstExpr::get_dependency() const +{ + return {}; +} + +LinkExpr::LinkExpr(ExprInit & exprs, ConfigObject & config) +{ + // TODO(Takagi, Isamu): remove + (void)config; + (void)exprs; +} + +void LinkExpr::init(ConfigObject & config, std::unordered_map nodes) +{ + const auto path = config.take_text("path"); + if (!nodes.count(path)) { + throw ConfigError("node path '" + path + "' does not exist"); + } + node_ = nodes.at(path); +} + +ExprStatus LinkExpr::eval() const +{ + ExprStatus status; + status.level = node_->level(); + status.links.push_back(std::make_pair(node_, true)); + return status; +} + +std::vector LinkExpr::get_dependency() const +{ + return {node_}; +} + +AndExpr::AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit) +{ + list_ = create_expr_list(exprs, config); + short_circuit_ = short_circuit; +} + +ExprStatus AndExpr::eval() const +{ + if (list_.empty()) { + ExprStatus status; + status.level = DiagnosticStatus::OK; + return status; + } + + ExprStatus status; + status.level = DiagnosticStatus::OK; + for (const auto & expr : list_) { + const auto result = expr->eval(); + status.level = std::max(status.level, result.level); + extend(status.links, result.links); + if (short_circuit_ && status.level != DiagnosticStatus::OK) { + break; + } + } + status.level = std::min(status.level, DiagnosticStatus::ERROR); + return status; +} + +std::vector AndExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +OrExpr::OrExpr(ExprInit & exprs, ConfigObject & config) +{ + list_ = create_expr_list(exprs, config); +} + +ExprStatus OrExpr::eval() const +{ + if (list_.empty()) { + ExprStatus status; + status.level = DiagnosticStatus::OK; + return status; + } + + ExprStatus status; + status.level = DiagnosticStatus::ERROR; + for (const auto & expr : list_) { + const auto result = expr->eval(); + status.level = std::min(status.level, result.level); + extend(status.links, result.links); + } + status.level = std::min(status.level, DiagnosticStatus::ERROR); + return status; +} + +std::vector OrExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +ExprInit::ExprInit(const std::string & mode) +{ + mode_ = mode; +} + +std::unique_ptr ExprInit::create(ExprConfig config) +{ + if (!config.mode.check(mode_)) { + return nullptr; + } + if (config.type == "link") { + auto expr = std::make_unique(*this, config.dict); + links_.push_back(std::make_pair(expr.get(), config.dict)); + return expr; + } + if (config.type == "and") { + return std::make_unique(*this, config.dict, false); + } + if (config.type == "short-circuit-and") { + return std::make_unique(*this, config.dict, true); + } + if (config.type == "or") { + return std::make_unique(*this, config.dict); + } + if (config.type == "debug-ok") { + return std::make_unique(DiagnosticStatus::OK); + } + if (config.type == "debug-warn") { + return std::make_unique(DiagnosticStatus::WARN); + } + if (config.type == "debug-error") { + return std::make_unique(DiagnosticStatus::ERROR); + } + if (config.type == "debug-stale") { + return std::make_unique(DiagnosticStatus::STALE); + } + throw ConfigError("unknown expr type: " + config.type + " " + config.dict.mark().str()); +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/expr.hpp b/system/system_diagnostic_graph/src/core/exprs.hpp similarity index 68% rename from system/system_diagnostic_graph/src/core/expr.hpp rename to system/system_diagnostic_graph/src/core/exprs.hpp index 541841582180a..f582e019d5691 100644 --- a/system/system_diagnostic_graph/src/core/expr.hpp +++ b/system/system_diagnostic_graph/src/core/exprs.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CORE__EXPR_HPP_ -#define CORE__EXPR_HPP_ +#ifndef CORE__EXPRS_HPP_ +#define CORE__EXPRS_HPP_ +#include "config.hpp" #include "types.hpp" -#include - #include #include +#include #include #include @@ -36,7 +36,6 @@ struct ExprStatus class BaseExpr { public: - static std::unique_ptr create(Graph & graph, YAML::Node yaml); virtual ~BaseExpr() = default; virtual ExprStatus eval() const = 0; virtual std::vector get_dependency() const = 0; @@ -53,43 +52,34 @@ class ConstExpr : public BaseExpr DiagnosticLevel level_; }; -class UnitExpr : public BaseExpr +class LinkExpr : public BaseExpr { public: - UnitExpr(Graph & graph, YAML::Node yaml); + LinkExpr(ExprInit & exprs, ConfigObject & config); + void init(ConfigObject & config, std::unordered_map nodes); ExprStatus eval() const override; std::vector get_dependency() const override; private: - UnitNode * node_; -}; - -class DiagExpr : public BaseExpr -{ -public: - DiagExpr(Graph & graph, YAML::Node yaml); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - DiagNode * node_; + BaseNode * node_; }; class AndExpr : public BaseExpr { public: - AndExpr(Graph & graph, YAML::Node yaml); + AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit); ExprStatus eval() const override; std::vector get_dependency() const override; private: + bool short_circuit_; std::vector> list_; }; class OrExpr : public BaseExpr { public: - OrExpr(Graph & graph, YAML::Node yaml); + OrExpr(ExprInit & exprs, ConfigObject & config); ExprStatus eval() const override; std::vector get_dependency() const override; @@ -97,18 +87,18 @@ class OrExpr : public BaseExpr std::vector> list_; }; -class IfExpr : public BaseExpr +class ExprInit { public: - IfExpr(Graph & graph, YAML::Node yaml); - ExprStatus eval() const override; - std::vector get_dependency() const override; + explicit ExprInit(const std::string & mode); + std::unique_ptr create(ExprConfig config); + auto get() const { return links_; } private: - std::unique_ptr cond_; - std::unique_ptr then_; + std::string mode_; + std::vector> links_; }; } // namespace system_diagnostic_graph -#endif // CORE__EXPR_HPP_ +#endif // CORE__EXPRS_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index ba0e3cfd2e016..b4fd1d15827f3 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -14,58 +14,32 @@ #include "graph.hpp" -#include "node.hpp" +#include "config.hpp" +#include "exprs.hpp" +#include "nodes.hpp" #include -#include #include +#include +#include #include +#include -// +// DEBUG #include namespace system_diagnostic_graph { -UnitNode * Graph::make_unit(const std::string & name) +void topological_sort(std::vector> & input) { - const auto key = name; - auto unit = std::make_unique(key); - units_[key] = unit.get(); - nodes_.emplace_back(std::move(unit)); - return units_[key]; -} - -UnitNode * Graph::find_unit(const std::string & name) -{ - const auto key = name; - return units_.count(key) ? units_.at(key) : nullptr; -} - -DiagNode * Graph::make_diag(const std::string & name, const std::string & hardware) -{ - const auto key = std::make_pair(name, hardware); - auto diag = std::make_unique(name, hardware); - diags_[key] = diag.get(); - nodes_.emplace_back(std::move(diag)); - return diags_[key]; -} - -DiagNode * Graph::find_diag(const std::string & name, const std::string & hardware) -{ - const auto key = std::make_pair(name, hardware); - return diags_.count(key) ? diags_.at(key) : nullptr; -} - -void Graph::topological_sort() -{ - std::map degrees; + std::unordered_map degrees; std::deque nodes; std::deque result; std::deque buffer; // Create a list of raw pointer nodes. - for (const auto & node : nodes_) { + for (const auto & node : input) { nodes.push_back(node.get()); } @@ -104,15 +78,127 @@ void Graph::topological_sort() result = std::deque(result.rbegin(), result.rend()); // Replace node vector. - std::map indices; + std::unordered_map indices; for (size_t i = 0; i < result.size(); ++i) { indices[result[i]] = i; } - std::vector> temp(nodes_.size()); - for (auto & node : nodes_) { - temp[indices[node.get()]] = std::move(node); + std::vector> sorted(input.size()); + for (auto & node : input) { + sorted[indices[node.get()]] = std::move(node); + } + input = std::move(sorted); +} + +Graph::Graph() +{ + // for unique_ptr +} + +Graph::~Graph() +{ + // for unique_ptr +} + +void Graph::init(const std::string & file, const std::string & mode) +{ + ConfigFile root = load_config_root(file); + + std::vector> nodes; + std::unordered_map diags; + std::unordered_map paths; + ExprInit exprs(mode); + + for (auto & config : root.units) { + if (config.mode.check(mode)) { + auto node = std::make_unique(config.path); + nodes.push_back(std::move(node)); + } + } + for (auto & config : root.diags) { + if (config.mode.check(mode)) { + auto node = std::make_unique(config.path, config.dict); + diags[node->name()] = node.get(); + nodes.push_back(std::move(node)); + } + } + + // TODO(Takagi, Isamu): unknown diag names + { + auto node = std::make_unique("/unknown"); + unknown_ = node.get(); + nodes.push_back(std::move(node)); + } + + for (const auto & node : nodes) { + paths[node->path()] = node.get(); + } + + for (auto & config : root.units) { + if (paths.count(config.path)) { + paths.at(config.path)->create(config.dict, exprs); + } + } + for (auto & config : root.diags) { + if (paths.count(config.path)) { + paths.at(config.path)->create(config.dict, exprs); + } + } + + for (auto & [link, config] : exprs.get()) { + link->init(config, paths); + } + + // Sort unit nodes in topological order for update dependencies. + topological_sort(nodes); + + // Set the link index for the ros message. + for (size_t i = 0; i < nodes.size(); ++i) { + nodes[i]->set_index(i); + } + + nodes_ = std::move(nodes); + diags_ = diags; +} + +void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) +{ + for (const auto & status : array.status) { + const auto iter = diags_.find(status.name); + if (iter != diags_.end()) { + iter->second->callback(status, stamp); + } else { + unknown_->callback(status, stamp); + } + } +} + +void Graph::update(const rclcpp::Time & stamp) +{ + for (const auto & node : nodes_) { + node->update(stamp); + } + stamp_ = stamp; +} + +DiagnosticGraph Graph::message() const +{ + DiagnosticGraph result; + result.stamp = stamp_; + result.nodes.reserve(nodes_.size()); + for (const auto & node : nodes_) { + result.nodes.push_back(node->report()); + } + return result; +} + +std::vector Graph::nodes() const +{ + std::vector result; + result.reserve(nodes_.size()); + for (const auto & node : nodes_) { + result.push_back(node.get()); } - nodes_ = std::move(temp); + return result; } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index a3f46760388f7..e0060c9111592 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -17,29 +17,36 @@ #include "types.hpp" -#include +#include + #include #include +#include #include #include namespace system_diagnostic_graph { -class Graph +class Graph final { public: - UnitNode * make_unit(const std::string & name); - UnitNode * find_unit(const std::string & name); - DiagNode * make_diag(const std::string & name, const std::string & hardware); - DiagNode * find_diag(const std::string & name, const std::string & hardware); - void topological_sort(); - const std::vector> & nodes() { return nodes_; } + Graph(); + ~Graph(); + + void init(const std::string & file, const std::string & mode); + void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); + void update(const rclcpp::Time & stamp); + DiagnosticGraph message() const; + std::vector nodes() const; + + void debug(); private: std::vector> nodes_; - std::map units_; - std::map, DiagNode *> diags_; + std::unordered_map diags_; + UnknownNode * unknown_; + rclcpp::Time stamp_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp new file mode 100644 index 0000000000000..0ca18f84b0407 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "modes.hpp" + +#include "config.hpp" +#include "nodes.hpp" + +#include +#include +#include + +namespace system_diagnostic_graph +{ + +OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) +{ + pub_ = node.create_publisher("/system/operation_mode/availability", rclcpp::QoS(1)); + + using PathNodes = std::unordered_map; + PathNodes paths; + for (const auto & node : graph) { + paths[node->path()] = node; + } + + const auto find_node = [](const PathNodes & paths, const std::string & name) { + const auto iter = paths.find(name); + if (iter != paths.end()) { + return iter->second; + } + throw ConfigError("summary node '" + name + "' does node exist"); + }; + + // clang-format off + stop_mode_ = find_node(paths, "/autoware/modes/stop"); + autonomous_mode_ = find_node(paths, "/autoware/modes/autonomous"); + local_mode_ = find_node(paths, "/autoware/modes/local"); + remote_mode_ = find_node(paths, "/autoware/modes/remote"); + emergency_stop_mrm_ = find_node(paths, "/autoware/modes/emergency-stop"); + comfortable_stop_mrm_ = find_node(paths, "/autoware/modes/comfortable-stop"); + pull_over_mrm_ = find_node(paths, "/autoware/modes/pull-over"); + // clang-format on +} + +void OperationModes::update(const rclcpp::Time & stamp) const +{ + const auto is_ok = [](const BaseNode * node) { return node->level() == DiagnosticStatus::OK; }; + + // clang-format off + Availability message; + message.stamp = stamp; + message.stop = is_ok(stop_mode_); + message.autonomous = is_ok(autonomous_mode_); + message.local = is_ok(local_mode_); + message.remote = is_ok(remote_mode_); + message.emergency_stop = is_ok(emergency_stop_mrm_); + message.comfortable_stop = is_ok(comfortable_stop_mrm_); + message.pull_over = is_ok(pull_over_mrm_); + // clang-format on + + pub_->publish(message); +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/update.hpp b/system/system_diagnostic_graph/src/core/modes.hpp similarity index 53% rename from system/system_diagnostic_graph/src/core/update.hpp rename to system/system_diagnostic_graph/src/core/modes.hpp index 3cba96ad8203a..ead1ec10dce93 100644 --- a/system/system_diagnostic_graph/src/core/update.hpp +++ b/system/system_diagnostic_graph/src/core/modes.hpp @@ -12,47 +12,40 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CORE__UPDATE_HPP_ -#define CORE__UPDATE_HPP_ +#ifndef CORE__MODES_HPP_ +#define CORE__MODES_HPP_ -#include "graph.hpp" -#include "node.hpp" #include "types.hpp" #include -#include +#include + #include namespace system_diagnostic_graph { -struct Summary -{ - UnitNode * stop_mode; - UnitNode * autonomous_mode; - UnitNode * local_mode; - UnitNode * remote_mode; - UnitNode * emergency_stop_mrm; - UnitNode * comfortable_stop_mrm; - UnitNode * pull_over_mrm; -}; - -class DiagGraph +class OperationModes { public: - void create(const std::string & file); - void callback(const DiagnosticArray & array); - DiagnosticGraph report(const rclcpp::Time & stamp); - OperationModeAvailability summary(const rclcpp::Time & stamp); - - void debug(); + explicit OperationModes(rclcpp::Node & node, const std::vector & graph); + void update(const rclcpp::Time & stamp) const; private: - Graph graph_; - Summary summary_; + using Availability = tier4_system_msgs::msg::OperationModeAvailability; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_; + + BaseNode * stop_mode_; + BaseNode * autonomous_mode_; + BaseNode * local_mode_; + BaseNode * remote_mode_; + BaseNode * emergency_stop_mrm_; + BaseNode * comfortable_stop_mrm_; + BaseNode * pull_over_mrm_; }; } // namespace system_diagnostic_graph -#endif // CORE__UPDATE_HPP_ +#endif // CORE__MODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/node.cpp b/system/system_diagnostic_graph/src/core/node.cpp deleted file mode 100644 index 1439188d93e18..0000000000000 --- a/system/system_diagnostic_graph/src/core/node.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "node.hpp" - -#include "expr.hpp" - -#include - -#include -#include - -namespace system_diagnostic_graph -{ - -UnitNode::UnitNode(const std::string & name) -{ - node_.status.level = DiagnosticStatus::STALE; - node_.status.name = name; - node_.status.hardware_id = ""; -} - -UnitNode::~UnitNode() -{ - // for unique_ptr -} - -DiagnosticNode UnitNode::report() const -{ - return node_; -} - -void UnitNode::update() -{ - const auto result = expr_->eval(); - node_.status.level = result.level; - node_.links.clear(); - for (const auto & [node, used] : result.links) { - DiagnosticLink link; - link.index = node->index(); - link.used = used; - node_.links.push_back(link); - } -} - -void UnitNode::create(Graph & graph, const NodeConfig & config) -{ - try { - expr_ = BaseExpr::create(graph, config->yaml); - } catch (const ConfigError & error) { - throw create_error(config, error.what()); - } -} - -std::vector UnitNode::links() const -{ - return expr_->get_dependency(); -} - -DiagNode::DiagNode(const std::string & name, const std::string & hardware) -{ - node_.status.level = DiagnosticStatus::STALE; - node_.status.name = name; - node_.status.hardware_id = hardware; -} - -DiagnosticNode DiagNode::report() const -{ - return node_; -} - -void DiagNode::update() -{ - // TODO(Takagi, Isamu): timeout, error hold - // constexpr double timeout = 1.0; // TODO(Takagi, Isamu): parameterize -} - -void DiagNode::callback(const DiagnosticStatus & status) -{ - node_.status = status; -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/node.hpp b/system/system_diagnostic_graph/src/core/node.hpp deleted file mode 100644 index 359153fc2824a..0000000000000 --- a/system/system_diagnostic_graph/src/core/node.hpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CORE__NODE_HPP_ -#define CORE__NODE_HPP_ - -#include "config.hpp" -#include "debug.hpp" -#include "types.hpp" - -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -class BaseNode -{ -public: - virtual ~BaseNode() = default; - virtual void update() = 0; - virtual DiagnosticNode report() const = 0; - virtual DiagDebugData debug() const = 0; - virtual std::vector links() const = 0; - - DiagnosticLevel level() const { return node_.status.level; } - std::string name() const { return node_.status.name; } - - size_t index() const { return index_; } - void set_index(const size_t index) { index_ = index; } - -protected: - size_t index_ = 0; - DiagnosticNode node_; -}; - -class UnitNode : public BaseNode -{ -public: - explicit UnitNode(const std::string & name); - ~UnitNode() override; - - DiagnosticNode report() const override; - DiagDebugData debug() const override; - void update() override; - void create(Graph & graph, const NodeConfig & config); - - std::vector links() const override; - -private: - std::unique_ptr expr_; -}; - -class DiagNode : public BaseNode -{ -public: - explicit DiagNode(const std::string & name, const std::string & hardware); - - DiagnosticNode report() const override; - DiagDebugData debug() const override; - void update() override; - void callback(const DiagnosticStatus & status); - - std::vector links() const override { return {}; } - -private: -}; - -} // namespace system_diagnostic_graph - -#endif // CORE__NODE_HPP_ diff --git a/system/system_diagnostic_graph/src/core/nodes.cpp b/system/system_diagnostic_graph/src/core/nodes.cpp new file mode 100644 index 0000000000000..bbc4bb4d42561 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/nodes.cpp @@ -0,0 +1,157 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "nodes.hpp" + +#include "exprs.hpp" + +#include + +#include +#include + +namespace system_diagnostic_graph +{ + +BaseNode::BaseNode(const std::string & path) : path_(path) +{ + index_ = 0; +} + +UnitNode::UnitNode(const std::string & path) : BaseNode(path) +{ + level_ = DiagnosticStatus::STALE; +} + +UnitNode::~UnitNode() +{ + // for unique_ptr +} + +void UnitNode::create(ConfigObject & config, ExprInit & exprs) +{ + expr_ = exprs.create(parse_expr_config(config)); +} + +void UnitNode::update(const rclcpp::Time &) +{ + const auto result = expr_->eval(); + level_ = result.level; + links_.clear(); + for (const auto & [node, used] : result.links) { + DiagnosticLink link; + link.index = node->index(); + link.used = used; + links_.push_back(link); + } +} + +DiagnosticNode UnitNode::report() const +{ + DiagnosticNode message; + message.status.level = level_; + message.status.name = path_; + message.links = links_; + return message; +} + +DiagnosticLevel UnitNode::level() const +{ + return level_; +} + +std::vector UnitNode::links() const +{ + return expr_->get_dependency(); +} + +DiagNode::DiagNode(const std::string & path, ConfigObject & config) : BaseNode(path) +{ + timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize + name_ = config.take_text("name"); + + status_.level = DiagnosticStatus::STALE; +} + +void DiagNode::create(ConfigObject &, ExprInit &) +{ +} + +void DiagNode::update(const rclcpp::Time & stamp) +{ + if (time_) { + const auto elapsed = (stamp - time_.value()).seconds(); + if (timeout_ < elapsed) { + status_ = DiagnosticStatus(); + status_.level = DiagnosticStatus::STALE; + time_ = std::nullopt; + } + } +} + +DiagnosticNode DiagNode::report() const +{ + DiagnosticNode message; + message.status = status_; + message.status.name = path_; + return message; +} + +DiagnosticLevel DiagNode::level() const +{ + return status_.level; +} + +void DiagNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) +{ + status_ = status; + time_ = stamp; +} + +UnknownNode::UnknownNode(const std::string & path) : BaseNode(path) +{ +} + +void UnknownNode::create(ConfigObject &, ExprInit &) +{ +} + +void UnknownNode::update(const rclcpp::Time & stamp) +{ + (void)stamp; +} + +DiagnosticNode UnknownNode::report() const +{ + DiagnosticNode message; + message.status.name = path_; + for (const auto & diag : diagnostics_) { + diagnostic_msgs::msg::KeyValue kv; + kv.key = diag.first; + message.status.values.push_back(kv); + } + return message; +} + +DiagnosticLevel UnknownNode::level() const +{ + return DiagnosticStatus::WARN; +} + +void UnknownNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) +{ + diagnostics_[status.name] = stamp; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/nodes.hpp b/system/system_diagnostic_graph/src/core/nodes.hpp new file mode 100644 index 0000000000000..1a849cf58b268 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/nodes.hpp @@ -0,0 +1,114 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CORE__NODES_HPP_ +#define CORE__NODES_HPP_ + +#include "config.hpp" +#include "debug.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class BaseNode +{ +public: + explicit BaseNode(const std::string & path); + virtual ~BaseNode() = default; + virtual void create(ConfigObject & config, ExprInit & exprs) = 0; + virtual void update(const rclcpp::Time & stamp) = 0; + virtual DiagnosticNode report() const = 0; + virtual DiagnosticLevel level() const = 0; + virtual DiagDebugData debug() const = 0; + virtual std::vector links() const = 0; + + std::string path() const { return path_; } + + size_t index() const { return index_; } + void set_index(const size_t index) { index_ = index; } + +protected: + const std::string path_; + size_t index_; +}; + +class UnitNode : public BaseNode +{ +public: + explicit UnitNode(const std::string & path); + ~UnitNode() override; + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override; + +private: + std::unique_ptr expr_; + std::vector links_; + DiagnosticLevel level_; +}; + +class DiagNode : public BaseNode +{ +public: + DiagNode(const std::string & path, ConfigObject & config); + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override { return {}; } + std::string name() const { return name_; } + + void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); + +private: + double timeout_; + std::optional time_; + std::string name_; + DiagnosticStatus status_; +}; + +class UnknownNode : public BaseNode +{ +public: + explicit UnknownNode(const std::string & path); + void create(ConfigObject & config, ExprInit & exprs) override; + void update(const rclcpp::Time & stamp) override; + DiagnosticNode report() const override; + DiagnosticLevel level() const override; + DiagDebugData debug() const override; + std::vector links() const override { return {}; } + + void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); + +private: + std::map diagnostics_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__NODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/types.hpp b/system/system_diagnostic_graph/src/core/types.hpp index 75167958e49bc..2adfbb3f9d4ef 100644 --- a/system/system_diagnostic_graph/src/core/types.hpp +++ b/system/system_diagnostic_graph/src/core/types.hpp @@ -20,7 +20,6 @@ #include #include #include -#include namespace system_diagnostic_graph { @@ -30,15 +29,15 @@ using diagnostic_msgs::msg::DiagnosticStatus; using tier4_system_msgs::msg::DiagnosticGraph; using tier4_system_msgs::msg::DiagnosticLink; using tier4_system_msgs::msg::DiagnosticNode; -using tier4_system_msgs::msg::OperationModeAvailability; - using DiagnosticLevel = DiagnosticStatus::_level_type; -class Graph; class BaseNode; class UnitNode; class DiagNode; +class UnknownNode; + class BaseExpr; +class ExprInit; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/update.cpp b/system/system_diagnostic_graph/src/core/update.cpp deleted file mode 100644 index bb42dcba12192..0000000000000 --- a/system/system_diagnostic_graph/src/core/update.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "update.hpp" - -#include "config.hpp" - -#include -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -UnitNode * find_node(Graph & graph, const std::string & name) -{ - const auto node = graph.find_unit(name); - if (!node) { - throw ConfigError("summary node '" + name + "' does node exist"); - } - return node; -}; - -void DiagGraph::create(const std::string & file) -{ - const auto configs = load_config_file(file); - - // Create unit nodes first because it is necessary for the link. - std::vector> units; - for (const auto & config : configs) { - UnitNode * unit = graph_.make_unit(config->name); - units.push_back(std::make_pair(config, unit)); - } - - // Reflect the config after creating all the unit nodes, - for (auto & [config, unit] : units) { - unit->create(graph_, config); - } - - // Sort unit nodes in topological order for update dependencies. - graph_.topological_sort(); - - // Set the link index for the ros message. - const auto & nodes = graph_.nodes(); - for (size_t i = 0; i < nodes.size(); ++i) { - nodes[i]->set_index(i); - } - - // Get reserved unit node for summary. - summary_.stop_mode = find_node(graph_, "/autoware/operation/stop"); - summary_.autonomous_mode = find_node(graph_, "/autoware/operation/autonomous"); - summary_.local_mode = find_node(graph_, "/autoware/operation/local"); - summary_.remote_mode = find_node(graph_, "/autoware/operation/remote"); - summary_.emergency_stop_mrm = find_node(graph_, "/autoware/operation/emergency-stop"); - summary_.comfortable_stop_mrm = find_node(graph_, "/autoware/operation/comfortable-stop"); - summary_.pull_over_mrm = find_node(graph_, "/autoware/operation/pull-over"); -} - -DiagnosticGraph DiagGraph::report(const rclcpp::Time & stamp) -{ - DiagnosticGraph message; - message.stamp = stamp; - message.nodes.reserve(graph_.nodes().size()); - - for (const auto & node : graph_.nodes()) { - node->update(); - } - for (const auto & node : graph_.nodes()) { - message.nodes.push_back(node->report()); - } - return message; -} - -OperationModeAvailability DiagGraph::summary(const rclcpp::Time & stamp) -{ - const auto is_ok = [](const UnitNode * node) { return node->level() == DiagnosticStatus::OK; }; - - OperationModeAvailability message; - message.stamp = stamp; - message.stop = is_ok(summary_.stop_mode); - message.autonomous = is_ok(summary_.autonomous_mode); - message.local = is_ok(summary_.local_mode); - message.remote = is_ok(summary_.remote_mode); - message.emergency_stop = is_ok(summary_.emergency_stop_mrm); - message.comfortable_stop = is_ok(summary_.comfortable_stop_mrm); - message.pull_over = is_ok(summary_.pull_over_mrm); - return message; -} - -void DiagGraph::callback(const DiagnosticArray & array) -{ - for (const auto & status : array.status) { - auto diag = graph_.find_diag(status.name, status.hardware_id); - if (diag) { - diag->callback(status); - } else { - // TODO(Takagi, Isamu): handle unknown diagnostics - } - } -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 38b9fa5bacb3b..722ddcf833577 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -22,6 +22,19 @@ namespace system_diagnostic_graph MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") { + // Init diagnostics graph. + { + const auto file = declare_parameter("graph_file"); + const auto mode = declare_parameter("mode"); + graph_.init(file, mode); + graph_.debug(); + } + + // Init plugins + if (declare_parameter("mode_availability")) { + modes_ = std::make_unique(*this, graph_.nodes()); + } + // Init ros interface. { using std::placeholders::_1; @@ -31,32 +44,30 @@ MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") const auto callback = std::bind(&MainNode::on_diag, this, _1); sub_input_ = create_subscription("/diagnostics", qos_input, callback); pub_graph_ = create_publisher("/diagnostics_graph", qos_graph); - pub_modes_ = create_publisher( - "/system/operation_mode/availability", rclcpp::QoS(1)); const auto rate = rclcpp::Rate(declare_parameter("rate")); timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); } +} - // Init diagnostics graph. - { - const auto file = declare_parameter("graph_file"); - graph_.create(file); - graph_.debug(); - } +MainNode::~MainNode() +{ + // for unique_ptr } void MainNode::on_timer() { - const auto data = graph_.report(now()); + const auto stamp = now(); + graph_.update(stamp); graph_.debug(); - pub_graph_->publish(data); - pub_modes_->publish(graph_.summary(now())); + pub_graph_->publish(graph_.message()); + + if (modes_) modes_->update(stamp); } void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) { - graph_.callback(*msg); + graph_.callback(*msg, now()); } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/main.hpp b/system/system_diagnostic_graph/src/main.hpp index 4cc659c978611..6deb0518cd9d0 100644 --- a/system/system_diagnostic_graph/src/main.hpp +++ b/system/system_diagnostic_graph/src/main.hpp @@ -15,11 +15,14 @@ #ifndef MAIN_HPP_ #define MAIN_HPP_ +#include "core/graph.hpp" +#include "core/modes.hpp" #include "core/types.hpp" -#include "core/update.hpp" #include +#include + namespace system_diagnostic_graph { @@ -27,13 +30,14 @@ class MainNode : public rclcpp::Node { public: MainNode(); + ~MainNode(); private: - DiagGraph graph_; + Graph graph_; + std::unique_ptr modes_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::Subscription::SharedPtr sub_input_; rclcpp::Publisher::SharedPtr pub_graph_; - rclcpp::Publisher::SharedPtr pub_modes_; void on_timer(); void on_diag(const DiagnosticArray::ConstSharedPtr msg); }; diff --git a/system/system_diagnostic_graph/src/tool.hpp b/system/system_diagnostic_graph/src/tool.hpp index 11f6a2632386b..5ad19b8460c4b 100644 --- a/system/system_diagnostic_graph/src/tool.hpp +++ b/system/system_diagnostic_graph/src/tool.hpp @@ -15,7 +15,6 @@ #ifndef TOOL_HPP_ #define TOOL_HPP_ -#include "core/graph.hpp" #include "core/types.hpp" #include diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt deleted file mode 100644 index 909210f99d55e..0000000000000 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(system_diagnostic_monitor) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_package(INSTALL_TO_SHARE config launch script) diff --git a/system/system_diagnostic_monitor/README.md b/system/system_diagnostic_monitor/README.md deleted file mode 100644 index 8dccca34db8c5..0000000000000 --- a/system/system_diagnostic_monitor/README.md +++ /dev/null @@ -1 +0,0 @@ -# System diagnostic monitor diff --git a/system/system_diagnostic_monitor/config/autoware.yaml b/system/system_diagnostic_monitor/config/autoware.yaml deleted file mode 100644 index 892a5da851ba7..0000000000000 --- a/system/system_diagnostic_monitor/config/autoware.yaml +++ /dev/null @@ -1,74 +0,0 @@ -files: - - { package: system_diagnostic_monitor, path: config/map.yaml } - - { package: system_diagnostic_monitor, path: config/localization.yaml } - - { package: system_diagnostic_monitor, path: config/planning.yaml } - - { package: system_diagnostic_monitor, path: config/perception.yaml } - - { package: system_diagnostic_monitor, path: config/control.yaml } - - { package: system_diagnostic_monitor, path: config/vehicle.yaml } - - { package: system_diagnostic_monitor, path: config/system.yaml } - -nodes: - - name: /autoware - type: and - list: - - { type: unit, name: /autoware/operation/stop } - - { type: unit, name: /autoware/operation/autonomous } - - { type: unit, name: /autoware/operation/local } - - { type: unit, name: /autoware/operation/remote } - - { type: unit, name: /autoware/operation/emergency-stop } - - { type: unit, name: /autoware/operation/comfortable-stop } - - { type: unit, name: /autoware/operation/pull-over } - - - name: /autoware/operation/stop - type: debug-ok - - - name: /autoware/operation/autonomous - type: and - list: - - { type: unit, name: /autoware/map } - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - { type: unit, name: /autoware/control } - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/local - type: and - list: - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/remote - type: and - list: - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/emergency-stop - type: and - list: - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/comfortable-stop - type: and - list: - - { type: unit, name: /autoware/map } - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - { type: unit, name: /autoware/control } - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } - - - name: /autoware/operation/pull-over - type: and - list: - - { type: unit, name: /autoware/map } - - { type: unit, name: /autoware/localization } - - { type: unit, name: /autoware/planning } - - { type: unit, name: /autoware/perception } - - { type: unit, name: /autoware/control } - - { type: unit, name: /autoware/vehicle } - - { type: unit, name: /autoware/system } diff --git a/system/system_diagnostic_monitor/config/control.yaml b/system/system_diagnostic_monitor/config/control.yaml deleted file mode 100644 index 8884a79847c71..0000000000000 --- a/system/system_diagnostic_monitor/config/control.yaml +++ /dev/null @@ -1,10 +0,0 @@ -nodes: - - name: /autoware/control - type: and - list: - - type: diag - name: "topic_state_monitor_trajectory_follower_control_cmd: control_topic_status" - hardware: topic_state_monitor - - type: diag - name: "topic_state_monitor_control_command_control_cmd: control_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/localization.yaml b/system/system_diagnostic_monitor/config/localization.yaml deleted file mode 100644 index 26d680b6c4f0f..0000000000000 --- a/system/system_diagnostic_monitor/config/localization.yaml +++ /dev/null @@ -1,5 +0,0 @@ -nodes: - - name: /autoware/localization - type: and - list: - - { type: diag, name: "component_state_diagnostics: localization_state" } diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml deleted file mode 100644 index 7bee7419200bd..0000000000000 --- a/system/system_diagnostic_monitor/config/map.yaml +++ /dev/null @@ -1,7 +0,0 @@ -nodes: - - name: /autoware/map - type: and - list: - - type: diag - name: "topic_state_monitor_vector_map: map_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/perception.yaml b/system/system_diagnostic_monitor/config/perception.yaml deleted file mode 100644 index b6b4ec27d5596..0000000000000 --- a/system/system_diagnostic_monitor/config/perception.yaml +++ /dev/null @@ -1,7 +0,0 @@ -nodes: - - name: /autoware/perception - type: and - list: - - type: diag - name: "topic_state_monitor_object_recognition_objects: perception_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/planning.yaml b/system/system_diagnostic_monitor/config/planning.yaml deleted file mode 100644 index c29059193081b..0000000000000 --- a/system/system_diagnostic_monitor/config/planning.yaml +++ /dev/null @@ -1,15 +0,0 @@ -nodes: - - name: /autoware/planning - type: if - cond: - type: diag - name: "component_state_diagnostics: route_state" - then: - type: and - list: - - type: diag - name: "topic_state_monitor_mission_planning_route: planning_topic_status" - hardware: topic_state_monitor - - type: diag - name: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/system.yaml b/system/system_diagnostic_monitor/config/system.yaml deleted file mode 100644 index 0377e68daaae6..0000000000000 --- a/system/system_diagnostic_monitor/config/system.yaml +++ /dev/null @@ -1,7 +0,0 @@ -nodes: - - name: /autoware/system - type: and - list: - - type: diag - name: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/vehicle.yaml b/system/system_diagnostic_monitor/config/vehicle.yaml deleted file mode 100644 index 4826c96e5f72f..0000000000000 --- a/system/system_diagnostic_monitor/config/vehicle.yaml +++ /dev/null @@ -1,10 +0,0 @@ -nodes: - - name: /autoware/vehicle - type: and - list: - - type: diag - name: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" - hardware: topic_state_monitor - - type: diag - name: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" - hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml deleted file mode 100644 index a13b1dc9b78bc..0000000000000 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/system/system_diagnostic_monitor/package.xml b/system/system_diagnostic_monitor/package.xml deleted file mode 100644 index d410e75000876..0000000000000 --- a/system/system_diagnostic_monitor/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - system_diagnostic_monitor - 0.1.0 - The system_diagnostic_monitor package - Takagi, Isamu - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - - system_diagnostic_graph - - autoware_adapi_v1_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/system/system_diagnostic_monitor/script/component_state_diagnostics.py b/system/system_diagnostic_monitor/script/component_state_diagnostics.py deleted file mode 100755 index 8e12ed6656674..0000000000000 --- a/system/system_diagnostic_monitor/script/component_state_diagnostics.py +++ /dev/null @@ -1,79 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 The Autoware Contributors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from autoware_adapi_v1_msgs.msg import LocalizationInitializationState as LocalizationState -from autoware_adapi_v1_msgs.msg import RouteState -from diagnostic_msgs.msg import DiagnosticArray -from diagnostic_msgs.msg import DiagnosticStatus -import rclpy -import rclpy.node -import rclpy.qos - - -class ComponentStateDiagnostics(rclpy.node.Node): - def __init__(self): - super().__init__("component_state_diagnostics") - durable_qos = rclpy.qos.QoSProfile( - depth=1, - durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, - reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, - ) - - self.timer = self.create_timer(0.5, self.on_timer) - self.pub = self.create_publisher(DiagnosticArray, "/diagnostics", 1) - self.sub1 = self.create_subscription( - LocalizationState, - "/api/localization/initialization_state", - self.on_localization, - durable_qos, - ) - self.sub2 = self.create_subscription( - RouteState, "/api/routing/state", self.on_routing, durable_qos - ) - - self.diags = DiagnosticArray() - self.diags.status.append( - DiagnosticStatus( - level=DiagnosticStatus.STALE, name="component_state_diagnostics: localization_state" - ) - ) - self.diags.status.append( - DiagnosticStatus( - level=DiagnosticStatus.STALE, name="component_state_diagnostics: route_state" - ) - ) - - def on_timer(self): - self.diags.header.stamp = self.get_clock().now().to_msg() - self.pub.publish(self.diags) - - def on_localization(self, msg): - self.diags.status[0].level = ( - DiagnosticStatus.OK - if msg.state == LocalizationState.INITIALIZED - else DiagnosticStatus.ERROR - ) - - def on_routing(self, msg): - self.diags.status[1].level = ( - DiagnosticStatus.OK if msg.state != RouteState.UNSET else DiagnosticStatus.ERROR - ) - - -if __name__ == "__main__": - rclpy.init() - rclpy.spin(ComponentStateDiagnostics()) - rclpy.shutdown() From 992584bb6b03c04bf189567be055f67c962b038f Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Wed, 18 Oct 2023 17:32:25 +0900 Subject: [PATCH 123/206] chore(traffic_light_fine_detector): update onnx link in readme (#5339) chore(traffic_light_fine_detector): update onnx in readme Signed-off-by: Shunsuke Miura --- perception/traffic_light_fine_detector/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/perception/traffic_light_fine_detector/README.md b/perception/traffic_light_fine_detector/README.md index dcc89c76387c6..9e69c22fdc17b 100644 --- a/perception/traffic_light_fine_detector/README.md +++ b/perception/traffic_light_fine_detector/README.md @@ -16,7 +16,8 @@ The model was fine-tuned on around 17,000 TIER IV internal images of Japanese tr ### Trained Onnx model -- +You can download the ONNX file using these instructions. +Please visit [autoware-documentation](https://github.com/autowarefoundation/autoware-documentation/blob/main/docs/models/index.md) for more information. ## Inner-workings / Algorithms From 9a7fac5dd38294990a3a0be40f1837e349d38027 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 18 Oct 2023 18:12:18 +0900 Subject: [PATCH 124/206] fix(perception_reproducer): publish objects even if out of recorded timestamp (#5343) Signed-off-by: kosuke55 --- .../perception_replayer/perception_replayer_common.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 01079fdc3f8c1..f7453ad32ce58 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -153,6 +153,11 @@ def kill_online_perception_node(self): pass def binary_search(self, data, timestamp): + if data[-1][0] < timestamp: + return data[-1][1] + elif data[0][0] > timestamp: + return data[0][1] + low, high = 0, len(data) - 1 while low <= high: From a92b251516c0fdd2c8a7625fe725dfc25c046ba8 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 18 Oct 2023 18:39:10 +0900 Subject: [PATCH 125/206] perf(perception_reproducer): improve ego pose extraction (#5344) perf(perception_reproducer): imporove ego pose extraction Signed-off-by: kosuke55 --- .../perception_replayer/perception_replayer_common.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index f7453ad32ce58..7bf54c0278a27 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -180,10 +180,4 @@ def find_topics_by_timestamp(self, timestamp): return objects_data, traffic_signals_data def find_ego_odom_by_timestamp(self, timestamp): - ego_odom_data = None - for data in self.rosbag_ego_odom_data: - if timestamp < data[0]: - ego_odom_data = data[1] - break - - return ego_odom_data + return self.binary_search(self.rosbag_ego_odom_data, timestamp) From c6278ceace794c01cdd6bdefc460a90a77793167 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 18 Oct 2023 19:02:06 +0900 Subject: [PATCH 126/206] fix(lane_change): fix debug marker (#5346) Signed-off-by: kosuke55 --- .../path_safety_checker/safety_check.cpp | 26 ++++++++----------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index e36115c62e148..08ebd01ba5d05 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -157,7 +157,7 @@ Polygon2d createExtendedPolygon( { debug.forward_lon_offset = forward_lon_offset; debug.backward_lon_offset = backward_lon_offset; - debug.lat_offset = (left_lat_offset + right_lat_offset) / 2; + debug.lat_offset = std::max(std::abs(left_lat_offset), std::abs(right_lat_offset)); } const auto p1 = @@ -324,17 +324,15 @@ std::vector getCollidedPolygons( const auto & ego_polygon = interpolated_data->poly; const auto & ego_velocity = interpolated_data->velocity; - { - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = obj_pose; - debug.extended_ego_polygon = ego_polygon; - debug.extended_obj_polygon = obj_polygon; - } - // check overlap if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.unsafe_reason = "overlap_polygon"; collided_polygons.push_back(obj_polygon); + + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.extended_ego_polygon = ego_polygon; + debug.extended_obj_polygon = obj_polygon; continue; } @@ -366,19 +364,17 @@ std::vector getCollidedPolygons( : createExtendedPolygon( obj_pose, target_object.shape, lon_offset, lat_margin, is_stopped_object, debug); - { + // check overlap with extended polygon + if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { + debug.unsafe_reason = "overlap_extended_polygon"; + collided_polygons.push_back(obj_polygon); + debug.rss_longitudinal = rss_dist; debug.inter_vehicle_distance = min_lon_length; debug.extended_ego_polygon = extended_ego_polygon; debug.extended_obj_polygon = extended_obj_polygon; debug.is_front = is_object_front; } - - // check overlap with extended polygon - if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { - debug.unsafe_reason = "overlap_extended_polygon"; - collided_polygons.push_back(obj_polygon); - } } return collided_polygons; From 8530b6bb4155b040f8ff845ec030aa836218b94e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 18 Oct 2023 21:04:44 +0900 Subject: [PATCH 127/206] feat(pid_longitudinal_controller): transit to STOPPED even if the sign of the ego velocity goes opposite (#5255) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(pid_longitudinal_controller): transit to STOPPED even if the sign of the ego velocity goes opposite Signed-off-by: Takayuki Murooka * pid_longitudinal_controller.cpp ã‚’æ›´æ–° Co-authored-by: Takamasa Horibe * fix Signed-off-by: Takayuki Murooka --------- Signed-off-by: Takayuki Murooka Co-authored-by: Takamasa Horibe --- .../src/pid_longitudinal_controller.cpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index b80874a8a2e57..a134fd775155b 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -501,9 +501,26 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; - if ( - std::fabs(current_vel) > p.stopped_state_entry_vel || - std::fabs(current_acc) > p.stopped_state_entry_acc) { + + const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel && + std::abs(current_acc) < p.stopped_state_entry_acc; + // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also + // considered as a stop + const bool is_not_running = [&]() { + if (control_data.shift == Shift::Forward) { + if (is_stopped || current_vel < 0.0) { + // NOTE: Stopped or moving backward + return true; + } + } else { + if (is_stopped || 0.0 < current_vel) { + // NOTE: Stopped or moving forward + return true; + } + } + return false; + }(); + if (!is_not_running) { m_last_running_time = std::make_shared(clock_->now()); } const bool stopped_condition = From 89db33569a0b9a5e01c752b69dc3f4014db1be15 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 18 Oct 2023 21:38:00 +0900 Subject: [PATCH 128/206] feat(lane_change): disable cancel when ego is out of current lanes (#5348) --- .../src/scene_module/lane_change/interface.cpp | 2 +- .../src/scene_module/lane_change/normal.cpp | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 51875952d40fb..0a0cc3436bce7 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -147,7 +147,7 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - if (module_type_->isEgoOnPreparePhase()) { + if (module_type_->isEgoOnPreparePhase() && module_type_->isAbleToReturnCurrentLane()) { RCLCPP_WARN_STREAM_THROTTLE( getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, "Lane change path is unsafe. Cancel lane change."); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index ce07cda882692..de4afc9260da0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -510,6 +510,12 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + if (!utils::isEgoWithinOriginalLane( + status_.current_lanes, getEgoPose(), planner_data_->parameters, + lane_change_parameters_->cancel.overhang_tolerance)) { + return false; + } + const auto nearest_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, From 2ad1d816fa1bd31e0976fc9679c98ae2a9d56d4b Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Wed, 18 Oct 2023 23:23:25 +0900 Subject: [PATCH 129/206] perf(drivable area expansion): use faster lateral offset and nearest line calculations (#5349) Signed-off-by: Maxime CLEMENT --- .../drivable_area_expansion.hpp | 5 ++- .../drivable_area_expansion/map_utils.hpp | 6 ++-- .../utils/drivable_area_expansion/types.hpp | 4 +++ .../drivable_area_expansion.cpp | 35 +++++++++++-------- .../drivable_area_expansion/map_utils.cpp | 14 +++++--- 5 files changed, 39 insertions(+), 25 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp index 19ea89a3ce3c7..3f6f107cdce51 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -68,14 +68,13 @@ void apply_bound_change_rate_limit( /// @brief calculate the maximum distance by which a bound can be expanded /// @param [in] path_poses input path /// @param [in] bound bound points -/// @param [in] uncrossable_lines lines that limit the bound expansion +/// @param [in] uncrossable_segments segments that limit the bound expansion, indexed in a Rtree /// @param [in] uncrossable_polygons polygons that limit the bound expansion /// @param [in] params parameters with the buffer distance to keep with lines, /// and the static maximum expansion distance std::vector calculate_maximum_distance( const std::vector & path_poses, const std::vector bound, - const std::vector & uncrossable_lines, - const std::vector & uncrossable_polygons, + const SegmentRtree & uncrossable_lines, const std::vector & uncrossable_polygons, const DrivableAreaExpansionParameters & params); /// @brief expand a bound by the given lateral distances away from the path diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp index 6f96b83237310..8c6bdb8a6943b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp @@ -25,12 +25,12 @@ namespace drivable_area_expansion { -/// @brief Extract uncrossable linestrings from the lanelet map that are in range of ego +/// @brief Extract uncrossable segments from the lanelet map that are in range of ego /// @param[in] lanelet_map lanelet map /// @param[in] ego_point point of the current ego position /// @param[in] params parameters with linestring types that cannot be crossed and maximum range -/// @return the uncrossable linestrings -MultiLineString2d extract_uncrossable_lines( +/// @return the uncrossable segments stored in a rtree +SegmentRtree extract_uncrossable_segments( const lanelet::LaneletMap & lanelet_map, const Point & ego_point, const DrivableAreaExpansionParameters & params); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index 7db92c163f567..8da1521db6c28 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -22,6 +22,8 @@ #include #include +#include + namespace drivable_area_expansion { using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -39,6 +41,8 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::Segment2d; +typedef boost::geometry::index::rtree> SegmentRtree; + struct PointDistance { Point2d point; diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 25cf917d27135..3008f98331c92 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -36,12 +36,10 @@ namespace drivable_area_expansion namespace { - Point2d convert_point(const Point & p) { return Point2d{p.x, p.y}; } - } // namespace void reuse_previous_poses( @@ -61,11 +59,17 @@ void reuse_previous_poses( const auto deviation = motion_utils::calcLateralOffset(prev_poses, path.points.front().point.pose.position); if (first_idx && deviation < params.max_reuse_deviation) { + LineString2d path_ls; + for (const auto & p : path.points) path_ls.push_back(convert_point(p.point.pose.position)); for (auto idx = *first_idx; idx < prev_poses.size(); ++idx) { - if ( - motion_utils::calcLateralOffset(path.points, prev_poses[idx].position) > - params.max_reuse_deviation) - break; + double lateral_offset = std::numeric_limits::max(); + for (auto segment_idx = 0LU; segment_idx + 1 < path_ls.size(); ++segment_idx) { + const auto projection = point_to_line_projection( + convert_point(prev_poses[idx].position), path_ls[segment_idx], + path_ls[segment_idx + 1]); + lateral_offset = std::min(projection.distance, lateral_offset); + } + if (lateral_offset > params.max_reuse_deviation) break; cropped_poses.push_back(prev_poses[idx]); cropped_curvatures.push_back(prev_curvatures[idx]); } @@ -172,8 +176,7 @@ void apply_bound_change_rate_limit( std::vector calculate_maximum_distance( const std::vector & path_poses, const std::vector bound, - const std::vector & uncrossable_lines, - const std::vector & uncrossable_polygons, + const SegmentRtree & uncrossable_segments, const std::vector & uncrossable_polygons, const DrivableAreaExpansionParameters & params) { // TODO(Maxime): improve performances (dont use bg::distance ? use rtree ?) @@ -183,9 +186,13 @@ std::vector calculate_maximum_distance( for (const auto & p : bound) bound_ls.push_back(convert_point(p)); for (const auto & p : path_poses) path_ls.push_back(convert_point(p.position)); for (auto i = 0UL; i + 1 < bound_ls.size(); ++i) { - const LineString2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; - for (const auto & uncrossable_line : uncrossable_lines) { - const auto bound_to_line_dist = boost::geometry::distance(segment_ls, uncrossable_line); + const Segment2d segment_ls = {bound_ls[i], bound_ls[i + 1]}; + std::vector query_result; + boost::geometry::index::query( + uncrossable_segments, boost::geometry::index::nearest(segment_ls, 1), + std::back_inserter(query_result)); + if (!query_result.empty()) { + const auto bound_to_line_dist = boost::geometry::distance(segment_ls, query_result.front()); const auto dist_limit = std::max(0.0, bound_to_line_dist - params.avoid_linestring_dist); maximum_distances[i] = std::min(maximum_distances[i], dist_limit); maximum_distances[i + 1] = std::min(maximum_distances[i + 1], dist_limit); @@ -267,7 +274,7 @@ void expand_drivable_area( // crop first/last non deviating path_poses const auto & params = planner_data->drivable_area_expansion_parameters; const auto & route_handler = *planner_data->route_handler; - const auto uncrossable_lines = extract_uncrossable_lines( + const auto uncrossable_segments = extract_uncrossable_segments( *route_handler.getLaneletMapPtr(), planner_data->self_odometry->pose.pose.position, params); const auto uncrossable_polygons = create_object_footprints(*planner_data->dynamic_object, params); const auto preprocessing_ms = stop_watch.toc("preprocessing"); @@ -295,9 +302,9 @@ void expand_drivable_area( stop_watch.tic("max_dist"); const auto max_left_expansions = calculate_maximum_distance( - path_poses, path.left_bound, uncrossable_lines, uncrossable_polygons, params); + path_poses, path.left_bound, uncrossable_segments, uncrossable_polygons, params); const auto max_right_expansions = calculate_maximum_distance( - path_poses, path.right_bound, uncrossable_lines, uncrossable_polygons, params); + path_poses, path.right_bound, uncrossable_segments, uncrossable_polygons, params); for (auto i = 0LU; i < left_expansions.size(); ++i) left_expansions[i] = std::min(left_expansions[i], max_left_expansions[i]); for (auto i = 0LU; i < right_expansions.size(); ++i) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp index deeb787cf39f6..6ed14138c62e4 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp @@ -24,22 +24,26 @@ namespace drivable_area_expansion { -MultiLineString2d extract_uncrossable_lines( +SegmentRtree extract_uncrossable_segments( const lanelet::LaneletMap & lanelet_map, const Point & ego_point, const DrivableAreaExpansionParameters & params) { - MultiLineString2d uncrossable_lines_in_range; + SegmentRtree uncrossable_segments_in_range; LineString2d line; const auto ego_p = Point2d{ego_point.x, ego_point.y}; for (const auto & ls : lanelet_map.lineStringLayer) { if (has_types(ls, params.avoid_linestring_types)) { line.clear(); for (const auto & p : ls) line.push_back(Point2d{p.x(), p.y()}); - if (boost::geometry::distance(line, ego_p) < params.max_path_arc_length) - uncrossable_lines_in_range.push_back(line); + for (auto segment_idx = 0LU; segment_idx + 1 < line.size(); ++segment_idx) { + Segment2d segment = {line[segment_idx], line[segment_idx + 1]}; + if (boost::geometry::distance(segment, ego_p) < params.max_path_arc_length) { + uncrossable_segments_in_range.insert(segment); + } + } } } - return uncrossable_lines_in_range; + return uncrossable_segments_in_range; } bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types) From b0196439a149318a10f4cb3a22d2ee7a541ff886 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 19 Oct 2023 10:18:31 +0900 Subject: [PATCH 130/206] fix(goal_planner): mutex lock for all getter and setter of status (#5202) * fix(goal_planner): mutex lock for all getter and setter of status Signed-off-by: kosuke55 * use transaction instead of recursive_mutex Signed-off-by: kosuke55 fix Signed-off-by: kosuke55 * fix increment Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 175 +++++-- .../utils/goal_planner/util.hpp | 2 +- .../goal_planner/goal_planner_module.cpp | 476 ++++++++++-------- .../src/utils/goal_planner/util.cpp | 2 +- 4 files changed, 396 insertions(+), 259 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index a82cff16756f8..632872471ce29 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -74,28 +74,143 @@ enum class PathType { FREESPACE, }; -struct PullOverStatus +class PullOverStatus; // Forward declaration for Transaction +// class that locks the PullOverStatus when multiple values are being updated from +// an external source. +class Transaction { - std::shared_ptr pull_over_path{}; - std::shared_ptr lane_parking_pull_over_path{}; - size_t current_path_idx{0}; - bool require_increment_{true}; // if false, keep current path idx. - std::shared_ptr prev_stop_path{nullptr}; - std::shared_ptr prev_stop_path_after_approval{nullptr}; - // stop path after approval, stop path is not updated until safety is confirmed - lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain - lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain - std::vector lanes{}; // current + pull_over - bool has_decided_path{false}; // if true, the path has is decided and safe against static objects - bool is_safe_static_objects{false}; // current path is safe against *static* objects - bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects - bool prev_is_safe{false}; - bool prev_is_safe_dynamic_objects{false}; - bool has_decided_velocity{false}; - bool has_requested_approval{false}; - bool is_ready{false}; +public: + explicit Transaction(PullOverStatus & status); + ~Transaction(); + +private: + PullOverStatus & status_; }; +#define DEFINE_SETTER_GETTER(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + if (!is_in_transaction_) { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } else { \ + NAME##_ = value; \ + } \ + } \ + \ + TYPE get_##NAME() const \ + { \ + if (!is_in_transaction_) { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } \ + return NAME##_; \ + } + +class PullOverStatus +{ +public: + // Reset all data members to their initial states + void reset() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + lane_parking_pull_over_path_ = nullptr; + current_path_idx_ = 0; + require_increment_ = true; + prev_stop_path_ = nullptr; + prev_stop_path_after_approval_ = nullptr; + current_lanes_.clear(); + pull_over_lanes_.clear(); + lanes_.clear(); + has_decided_path_ = false; + is_safe_static_objects_ = false; + is_safe_dynamic_objects_ = false; + prev_is_safe_ = false; + prev_is_safe_dynamic_objects_ = false; + has_decided_velocity_ = false; + has_requested_approval_ = false; + is_ready_ = false; + } + + // lock all data members + Transaction startTransaction() { return Transaction(*this); } + + DEFINE_SETTER_GETTER(std::shared_ptr, pull_over_path) + DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) + DEFINE_SETTER_GETTER(size_t, current_path_idx) + DEFINE_SETTER_GETTER(bool, require_increment) + DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path) + DEFINE_SETTER_GETTER(std::shared_ptr, prev_stop_path_after_approval) + DEFINE_SETTER_GETTER(lanelet::ConstLanelets, current_lanes) + DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) + DEFINE_SETTER_GETTER(std::vector, lanes) + DEFINE_SETTER_GETTER(bool, has_decided_path) + DEFINE_SETTER_GETTER(bool, is_safe_static_objects) + DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) + DEFINE_SETTER_GETTER(bool, prev_is_safe) + DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) + DEFINE_SETTER_GETTER(bool, has_decided_velocity) + DEFINE_SETTER_GETTER(bool, has_requested_approval) + DEFINE_SETTER_GETTER(bool, is_ready) + DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) + DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_pose) + DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER(Pose, refined_goal_pose) + DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER(std::optional, closest_start_pose) + + void push_goal_candidate(const GoalCandidate & goal_candidate) + { + std::lock_guard lock(mutex_); + goal_candidates_.push_back(goal_candidate); + } + +private: + std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; + size_t current_path_idx_{0}; + bool require_increment_{true}; + std::shared_ptr prev_stop_path_{nullptr}; + std::shared_ptr prev_stop_path_after_approval_{nullptr}; + lanelet::ConstLanelets current_lanes_{}; + lanelet::ConstLanelets pull_over_lanes_{}; + std::vector lanes_{}; + bool has_decided_path_{false}; + bool is_safe_static_objects_{false}; + bool is_safe_dynamic_objects_{false}; + bool prev_is_safe_{false}; + bool prev_is_safe_dynamic_objects_{false}; + bool has_decided_velocity_{false}; + bool has_requested_approval_{false}; + bool is_ready_{false}; + + // save last time and pose + std::shared_ptr last_approved_time_; + std::shared_ptr last_increment_time_; + std::shared_ptr last_path_update_time_; + std::shared_ptr last_approved_pose_; + + // goal modification + std::optional modified_goal_pose_; + Pose refined_goal_pose_{}; + GoalCandidates goal_candidates_{}; + + // pull over path + std::vector pull_over_path_candidates_; + std::optional closest_start_pose_; + + friend class Transaction; + mutable std::mutex mutex_; + bool is_in_transaction_{false}; +}; + +#undef DEFINE_SETTER_GETTER + struct FreespacePlannerDebugData { bool is_planning{false}; @@ -155,8 +270,6 @@ class GoalPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override { return false; } - PullOverStatus status_; - mutable StartGoalPlannerData goal_planner_data_; std::shared_ptr parameters_; @@ -174,29 +287,19 @@ class GoalPlannerModule : public SceneModuleInterface // goal searcher std::shared_ptr goal_searcher_; - std::optional modified_goal_pose_; - Pose refined_goal_pose_{}; - GoalCandidates goal_candidates_{}; // collision detector // need to be shared_ptr to be used in planner and goal searcher std::shared_ptr occupancy_grid_map_; - // pull over path - std::vector pull_over_path_candidates_; - std::optional closest_start_pose_; - // check stopped and stuck state std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; tier4_autoware_utils::LinearRing2d vehicle_footprint_; - // save last time and pose - std::unique_ptr last_approved_time_; - std::unique_ptr last_increment_time_; - std::unique_ptr last_path_update_time_; - std::unique_ptr last_approved_pose_; + std::mutex mutex_; + PullOverStatus status_; // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. @@ -210,7 +313,6 @@ class GoalPlannerModule : public SceneModuleInterface // pre-generate lane parking paths in a separate thread rclcpp::TimerBase::SharedPtr lane_parking_timer_; rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_; - std::mutex mutex_; // generate freespace parking paths in a separate thread rclcpp::TimerBase::SharedPtr freespace_parking_timer_; @@ -245,7 +347,6 @@ class GoalPlannerModule : public SceneModuleInterface bool hasFinishedCurrentPath(); bool isOnModifiedGoal() const; double calcModuleRequestLength() const; - void resetStatus(); bool needPathUpdate(const double path_update_duration) const; bool isStuck(); bool hasDecidedPath() const; @@ -268,8 +369,8 @@ class GoalPlannerModule : public SceneModuleInterface BehaviorModuleOutput planWithGoalModification(); BehaviorModuleOutput planWaitingApprovalWithGoalModification(); void selectSafePullOverPath(); - void sortPullOverPathCandidatesByGoalPriority( - std::vector & pull_over_path_candidates, + std::vector sortPullOverPathCandidatesByGoalPriority( + const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const; // deal with pull over partial paths diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 1dc6cc411a31f..062a84bcd5aef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -63,7 +63,7 @@ MarkerArray createPosesMarkerArray( MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( - GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); + const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index c6e68f8e81af7..dc1ac1466397d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -51,6 +51,18 @@ using tier4_autoware_utils::inverseTransformPose; namespace behavior_path_planner { +Transaction::Transaction(PullOverStatus & status) : status_(status) +{ + status_.mutex_.lock(); + status_.is_in_transaction_ = true; +} + +Transaction::~Transaction() +{ + status_.mutex_.unlock(); + status_.is_in_transaction_ = false; +} + GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -113,16 +125,7 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } - resetStatus(); -} - -void GoalPlannerModule::resetStatus() -{ - PullOverStatus initial_status{}; - status_ = initial_status; - pull_over_path_candidates_.clear(); - closest_start_pose_.reset(); - goal_candidates_.clear(); + status_.reset(); } // This function is needed for waiting for planner_data_ @@ -139,17 +142,15 @@ void GoalPlannerModule::updateOccupancyGrid() void GoalPlannerModule::onTimer() { // already generated pull over candidate paths - if (!pull_over_path_candidates_.empty()) { + if (!status_.get_pull_over_path_candidates().empty()) { return; } // goals are not yet available. - if (goal_candidates_.empty()) { + if (status_.get_goal_candidates().empty()) { return; } - mutex_.lock(); - const auto goal_candidates = goal_candidates_; - mutex_.unlock(); + const auto goal_candidates = status_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( @@ -177,7 +178,6 @@ void GoalPlannerModule::onTimer() } } }; - // plan candidate paths and set them to the member variable if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { @@ -199,10 +199,9 @@ void GoalPlannerModule::onTimer() } // set member variables - mutex_.lock(); - pull_over_path_candidates_ = path_candidates; - closest_start_pose_ = closest_start_pose; - mutex_.unlock(); + const auto transaction = status_.startTransaction(); + status_.set_pull_over_path_candidates(path_candidates); + status_.set_closest_start_pose(closest_start_pose); } void GoalPlannerModule::onFreespaceParkingTimer() @@ -457,19 +456,23 @@ ModuleStatus GoalPlannerModule::updateState() bool GoalPlannerModule::planFreespacePath() { - mutex_.lock(); goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->update(goal_candidates_); - const auto goal_candidates = goal_candidates_; - debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); - debug_data_.freespace_planner.is_planning = true; - mutex_.unlock(); + auto goal_candidates = status_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + status_.set_goal_candidates(goal_candidates); + + { + const std::lock_guard lock(mutex_); + debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); + debug_data_.freespace_planner.is_planning = true; + } for (size_t i = 0; i < goal_candidates.size(); i++) { const auto goal_candidate = goal_candidates.at(i); - mutex_.lock(); - debug_data_.freespace_planner.current_goal_idx = i; - mutex_.unlock(); + { + const std::lock_guard lock(mutex_); + debug_data_.freespace_planner.current_goal_idx = i; + } if (!goal_candidate.is_safe) { continue; @@ -480,17 +483,22 @@ bool GoalPlannerModule::planFreespacePath() if (!freespace_path) { continue; } - mutex_.lock(); - status_.pull_over_path = std::make_shared(*freespace_path); - status_.current_path_idx = 0; - status_.is_safe_static_objects = true; - modified_goal_pose_ = goal_candidate; - last_path_update_time_ = std::make_unique(clock_->now()); - debug_data_.freespace_planner.is_planning = false; - mutex_.unlock(); + + { + const auto transaction = status_.startTransaction(); + status_.set_pull_over_path(std::make_shared(*freespace_path)); + status_.set_current_path_idx(0); + status_.set_is_safe_static_objects(true); + status_.set_modified_goal_pose(goal_candidate); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + const std::lock_guard lock(mutex_); + debug_data_.freespace_planner.is_planning = false; + } + return true; } + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.is_planning = false; return false; } @@ -502,11 +510,11 @@ void GoalPlannerModule::returnToLaneParking() return; } - if (!status_.lane_parking_pull_over_path) { + if (!status_.get_lane_parking_pull_over_path()) { return; } - const PathWithLaneId path = status_.lane_parking_pull_over_path->getFullPath(); + const PathWithLaneId path = status_.get_lane_parking_pull_over_path()->getFullPath(); if (checkCollision(path)) { return; } @@ -519,13 +527,14 @@ void GoalPlannerModule::returnToLaneParking() return; } - mutex_.lock(); - status_.is_safe_static_objects = true; - status_.has_decided_path = false; - status_.pull_over_path = status_.lane_parking_pull_over_path; - status_.current_path_idx = 0; - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + { + const auto transaction = status_.startTransaction(); + status_.set_is_safe_static_objects(true); + status_.set_has_decided_path(false); + status_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); + status_.set_current_path_idx(0); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + } RCLCPP_INFO(getLogger(), "return to lane parking"); } @@ -537,22 +546,22 @@ void GoalPlannerModule::generateGoalCandidates() // with old architecture, module instance is not cleared when new route is received // so need to reset status here. // todo: move this check out of this function after old architecture is removed - if (!goal_candidates_.empty()) { + if (!status_.get_goal_candidates().empty()) { return; } // calculate goal candidates const Pose goal_pose = route_handler->getOriginalGoalPose(); - refined_goal_pose_ = calcRefinedGoal(goal_pose); + status_.set_refined_goal_pose(calcRefinedGoal(goal_pose)); if (goal_planner_utils::isAllowedGoalModification(route_handler)) { goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->setReferenceGoal(refined_goal_pose_); - goal_candidates_ = goal_searcher_->search(); + goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); + status_.set_goal_candidates(goal_searcher_->search()); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; - goal_candidates_.push_back(goal_candidate); + status_.push_goal_candidate(goal_candidate); } } @@ -573,10 +582,12 @@ BehaviorModuleOutput GoalPlannerModule::plan() } } -void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( - std::vector & pull_over_path_candidates, +std::vector GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( + const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates) const { + auto sorted_pull_over_path_candidates = pull_over_path_candidates; + // Create a map of goal_id to its index in goal_candidates std::map goal_id_to_index; for (size_t i = 0; i < goal_candidates.size(); ++i) { @@ -585,7 +596,7 @@ void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( // Sort pull_over_path_candidates based on the order in goal_candidates std::stable_sort( - pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [&goal_id_to_index](const auto & a, const auto & b) { return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; }); @@ -593,7 +604,7 @@ void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( // Sort pull_over_path_candidates based on the order in efficient_path_order if (parameters_->path_priority == "efficient_path") { std::stable_sort( - pull_over_path_candidates.begin(), pull_over_path_candidates.end(), + sorted_pull_over_path_candidates.begin(), sorted_pull_over_path_candidates.end(), [this](const auto & a, const auto & b) { const auto & order = parameters_->efficient_path_order; const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); @@ -601,20 +612,27 @@ void GoalPlannerModule::sortPullOverPathCandidatesByGoalPriority( return a_pos < b_pos; }); } + + return sorted_pull_over_path_candidates; } void GoalPlannerModule::selectSafePullOverPath() { // select safe lane pull over path from candidates - mutex_.lock(); - goal_searcher_->setPlannerData(planner_data_); - goal_searcher_->update(goal_candidates_); - sortPullOverPathCandidatesByGoalPriority(pull_over_path_candidates_, goal_candidates_); - const auto pull_over_path_candidates = pull_over_path_candidates_; - const auto goal_candidates = goal_candidates_; - mutex_.unlock(); + std::vector pull_over_path_candidates{}; + GoalCandidates goal_candidates{}; + { + const auto transaction = status_.startTransaction(); + goal_searcher_->setPlannerData(planner_data_); + goal_candidates = status_.get_goal_candidates(); + goal_searcher_->update(goal_candidates); + status_.set_goal_candidates(goal_candidates); + status_.set_pull_over_path_candidates(sortPullOverPathCandidatesByGoalPriority( + status_.get_pull_over_path_candidates(), status_.get_goal_candidates())); + pull_over_path_candidates = status_.get_pull_over_path_candidates(); + status_.set_is_safe_static_objects(false); + } - status_.is_safe_static_objects = false; for (const auto & pull_over_path : pull_over_path_candidates) { // check if goal is safe const auto goal_candidate_it = std::find_if( @@ -633,71 +651,69 @@ void GoalPlannerModule::selectSafePullOverPath() continue; } - status_.is_safe_static_objects = true; - mutex_.lock(); - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.lane_parking_pull_over_path = status_.pull_over_path; - modified_goal_pose_ = *goal_candidate_it; - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + // found safe pull over path + { + const auto transaction = status_.startTransaction(); + status_.set_is_safe_static_objects(true); + status_.set_pull_over_path(std::make_shared(pull_over_path)); + status_.set_current_path_idx(0); + status_.set_lane_parking_pull_over_path(status_.get_pull_over_path()); + status_.set_modified_goal_pose(*goal_candidate_it); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + } break; } + if (!status_.get_is_safe_static_objects()) { + return; + } + // decelerate before the search area start - if (status_.is_safe_static_objects) { - const auto search_start_offset_pose = calcLongitudinalOffsetPose( - status_.pull_over_path->getFullPath().points, refined_goal_pose_.position, - -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - - approximate_pull_over_distance_); - auto & first_path = status_.pull_over_path->partial_paths.front(); - - if (search_start_offset_pose) { - decelerateBeforeSearchStart(*search_start_offset_pose, first_path); - } else { - // if already passed the search start offset pose, set pull_over_velocity to first_path. - const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - parameters_->pull_over_velocity); - for (auto & p : first_path.points) { - const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); - if (min_decel_distance && distance_from_ego < *min_decel_distance) { - continue; - } - p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); + const auto search_start_offset_pose = calcLongitudinalOffsetPose( + status_.get_pull_over_path()->getFullPath().points, status_.get_refined_goal_pose().position, + -parameters_->backward_goal_search_length - planner_data_->parameters.base_link2front - + approximate_pull_over_distance_); + auto & first_path = status_.get_pull_over_path()->partial_paths.front(); + if (search_start_offset_pose) { + decelerateBeforeSearchStart(*search_start_offset_pose, first_path); + } else { + // if already passed the search start offset pose, set pull_over_velocity to first_path. + const auto min_decel_distance = calcFeasibleDecelDistance( + planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, + parameters_->pull_over_velocity); + for (auto & p : first_path.points) { + const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); + if (min_decel_distance && distance_from_ego < *min_decel_distance) { + continue; } + p.point.longitudinal_velocity_mps = std::min( + p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); } } - - // generate drivable area for each partial path - for (auto & path : status_.pull_over_path->partial_paths) { - const size_t ego_idx = planner_data_->findEgoIndex(path.points); - utils::clipPathLength(path, ego_idx, planner_data_->parameters); - } } void GoalPlannerModule::setLanes() { - status_.current_lanes = utils::getExtendedCurrentLanes( + const auto transaction = status_.startTransaction(); + status_.set_current_lanes(utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, - /*forward_only_in_route*/ false); - status_.pull_over_lanes = goal_planner_utils::getPullOverLanes( + /*forward_only_in_route*/ false)); + status_.set_pull_over_lanes(goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); - status_.lanes = - utils::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_over_lanes); + parameters_->forward_goal_search_length)); + status_.set_lanes(utils::generateDrivableLanesWithShoulderLanes( + status_.get_current_lanes(), status_.get_pull_over_lanes())); } void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (!status_.is_safe_static_objects) { + if (!status_.get_is_safe_static_objects()) { // situation : not safe against static objects use stop_path setStopPath(output); } else if ( parameters_->safety_check_params.enable_safety_check && !isSafePath() && - status_.has_decided_path && isActivated()) { + status_.get_has_decided_path() && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -722,36 +738,37 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) setModifiedGoal(output); // set hazard and turn signal - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { setTurnSignalInfo(output); } // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - status_.prev_is_safe = status_.is_safe_static_objects; - status_.prev_is_safe_dynamic_objects = - parameters_->safety_check_params.enable_safety_check ? isSafePath() : true; + const auto transaction = status_.startTransaction(); + status_.set_prev_is_safe(status_.get_is_safe_static_objects()); + status_.set_prev_is_safe_dynamic_objects( + parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) { - if (status_.prev_is_safe || status_.prev_stop_path == nullptr) { + if (status_.get_prev_is_safe() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - status_.prev_stop_path = output.path; + const auto transaction = status_.startTransaction(); + status_.set_prev_stop_path(output.path); // set stop path as pull over path - mutex_.lock(); - PullOverPath pull_over_path{}; - status_.pull_over_path = std::make_shared(pull_over_path); - status_.current_path_idx = 0; - status_.pull_over_path->partial_paths.push_back(*output.path); - last_path_update_time_ = std::make_unique(clock_->now()); - mutex_.unlock(); + auto stop_pull_over_path = std::make_shared(); + stop_pull_over_path->partial_paths.push_back(*output.path); + status_.set_pull_over_path(stop_pull_over_path); + status_.set_current_path_idx(0); + status_.set_last_path_update_time(std::make_shared(clock_->now())); + RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path"); } else { // not_safe -> not_safe: use previous stop path - output.path = status_.prev_stop_path; + output.path = status_.get_prev_stop_path(); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } @@ -761,7 +778,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { + if (status_.get_prev_is_safe_dynamic_objects() || !status_.get_prev_stop_path_after_approval()) { auto current_path = getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -769,7 +786,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output parameters_->maximum_jerk_for_stop); if (stop_path) { output.path = std::make_shared(*stop_path); - status_.prev_stop_path_after_approval = output.path; + status_.set_prev_stop_path_after_approval(output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { output.path = std::make_shared(getCurrentPath()); @@ -777,11 +794,10 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output getLogger(), *clock_, 5000, "Collision detected, no feasible stop path found, cannot stop."); } - std::lock_guard lock(mutex_); - last_path_update_time_ = std::make_unique(clock_->now()); + status_.set_last_path_update_time(std::make_shared(clock_->now())); } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path - output.path = status_.prev_stop_path_after_approval; + output.path = status_.get_prev_stop_path_after_approval(); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } output.reference_path = getPreviousModuleOutput().reference_path; @@ -789,13 +805,13 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::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, status_.lanes, planner_data_->drivable_area_expansion_parameters); + *output.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -807,10 +823,10 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const void GoalPlannerModule::setModifiedGoal(BehaviorModuleOutput & output) const { const auto & route_handler = planner_data_->route_handler; - if (status_.is_safe_static_objects) { + if (status_.get_is_safe_static_objects()) { PoseWithUuidStamped modified_goal{}; modified_goal.uuid = route_handler->getRouteUuid(); - modified_goal.pose = modified_goal_pose_->goal_pose; + modified_goal.pose = status_.get_modified_goal_pose()->goal_pose; modified_goal.header = route_handler->getRouteHeader(); output.modified_goal = modified_goal; } else { @@ -850,12 +866,12 @@ void GoalPlannerModule::updateSteeringFactor( bool GoalPlannerModule::hasDecidedPath() const { // once decided, keep the decision - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { return true; } // if path is not safe, not decided - if (!status_.is_safe_static_objects) { + if (!status_.get_is_safe_static_objects()) { return false; } @@ -867,10 +883,10 @@ bool GoalPlannerModule::hasDecidedPath() const return false; } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - getCurrentPath().points, status_.pull_over_path->start_pose.position); + getCurrentPath().points, status_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( getCurrentPath().points, current_pose.position, *ego_segment_idx, - status_.pull_over_path->start_pose.position, start_pose_segment_idx); + status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); return dist_to_parking_start_pose < parameters_->decide_path_distance; } @@ -879,15 +895,15 @@ void GoalPlannerModule::decideVelocity() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; // decide velocity to guarantee turn signal lighting time - if (!status_.has_decided_velocity) { - auto & first_path = status_.pull_over_path->partial_paths.front(); + if (!status_.get_has_decided_velocity()) { + auto & first_path = status_.get_pull_over_path()->partial_paths.front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } } - status_.has_decided_velocity = true; + status_.set_has_decided_velocity(true); } BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() @@ -901,21 +917,26 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() setLanes(); // Check if it needs to decide path - status_.has_decided_path = hasDecidedPath(); + status_.set_has_decided_path(hasDecidedPath()); // Use decided path - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { if (isActivated() && isWaitingApproval()) { - last_approved_time_ = std::make_unique(clock_->now()); - last_approved_pose_ = std::make_unique(planner_data_->self_odometry->pose.pose); + { + const auto transaction = status_.startTransaction(); + status_.set_last_approved_time(std::make_shared(clock_->now())); + status_.set_last_approved_pose( + std::make_shared(planner_data_->self_odometry->pose.pose)); + } clearWaitingApproval(); decideVelocity(); } transitionToNextPathIfFinishingCurrentPath(); - } else if (!pull_over_path_candidates_.empty() && needPathUpdate(path_update_duration)) { + } else if ( + !status_.get_pull_over_path_candidates().empty() && needPathUpdate(path_update_duration)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates - // and set it to status_.pull_over_path + // and set it to status_.get_pull_over_path() selectSafePullOverPath(); } // else: stop path is generated and set by setOutput() @@ -923,21 +944,21 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.pull_over_path->getFullPath()); + path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { returnToLaneParking(); } const auto distance_to_path_change = calcDistanceToPathChange(); - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } // TODO(tkhmy) add handle status TRYING updateSteeringFactor( - {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, + {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::TURNING); // For debug @@ -947,7 +968,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() printParkingPositionError(); } - setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); return output; } @@ -976,20 +997,21 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = status_.is_safe_static_objects - ? std::make_shared(status_.pull_over_path->getFullPath()) - : out.path; + path_candidate_ = + status_.get_is_safe_static_objects() + ? std::make_shared(status_.get_pull_over_path()->getFullPath()) + : out.path; path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); // generate drivable area info for new architecture - if (status_.pull_over_path->type == PullOverPlannerType::FREESPACE) { + if (status_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; } else { const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes( - *out.path, status_.lanes, planner_data_->drivable_area_expansion_parameters); + *out.path, status_.get_lanes(), planner_data_->drivable_area_expansion_parameters); DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; @@ -997,21 +1019,21 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); } - if (status_.has_decided_path) { + if (status_.get_has_decided_path()) { updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); } updateSteeringFactor( - {status_.pull_over_path->start_pose, modified_goal_pose_->goal_pose}, + {status_.get_pull_over_path()->start_pose, status_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, status_.pull_over_path->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, status_.get_pull_over_path()->getFullPath()); return out; } std::pair GoalPlannerModule::calcDistanceToPathChange() const { - const auto & full_path = status_.pull_over_path->getFullPath(); + const auto & full_path = status_.get_pull_over_path()->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1021,15 +1043,15 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, status_.pull_over_path->start_pose.position); + full_path.points, status_.get_pull_over_path()->start_pose.position); const double dist_to_parking_start_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - status_.pull_over_path->start_pose.position, start_pose_segment_idx); + status_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); const size_t goal_pose_segment_idx = motion_utils::findNearestSegmentIndex( - full_path.points, modified_goal_pose_->goal_pose.position); + full_path.points, status_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( full_path.points, planner_data_->self_odometry->pose.pose.position, *ego_segment_idx, - modified_goal_pose_->goal_pose.position, goal_pose_segment_idx); + status_.get_modified_goal_pose()->goal_pose.position, goal_pose_segment_idx); return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } @@ -1047,17 +1069,17 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; const double pull_over_velocity = parameters_->pull_over_velocity; - if (status_.current_lanes.empty()) { + if (status_.get_current_lanes().empty()) { return PathWithLaneId{}; } // generate reference path const auto s_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; + lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; auto reference_path = - route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); // if not approved stop road lane. // stop point priority is @@ -1068,17 +1090,20 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // difference between the outer and inner sides) // 4. feasible stop const auto search_start_offset_pose = calcLongitudinalOffsetPose( - reference_path.points, refined_goal_pose_.position, + reference_path.points, status_.get_refined_goal_pose().position, -parameters_->backward_goal_search_length - common_parameters.base_link2front - approximate_pull_over_distance_); - if (!status_.is_safe_static_objects && !closest_start_pose_ && !search_start_offset_pose) { + if ( + !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && + !search_start_offset_pose) { return generateFeasibleStopPath(); } const Pose stop_pose = - status_.is_safe_static_objects - ? status_.pull_over_path->start_pose - : (closest_start_pose_ ? closest_start_pose_.value() : *search_start_offset_pose); + status_.get_is_safe_static_objects() + ? status_.get_pull_over_path()->start_pose + : (status_.get_closest_start_pose() ? status_.get_closest_start_pose().value() + : *search_start_offset_pose); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); @@ -1124,10 +1149,11 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() // generate stop reference path const auto s_current = - lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; + lanelet::utils::getArcCoordinates(status_.get_current_lanes(), current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; - auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + auto stop_path = + route_handler->getCenterLinePath(status_.get_current_lanes(), s_start, s_end, true); // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( @@ -1148,15 +1174,16 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && last_approved_time_ != nullptr) { + if (isActivated() && status_.get_last_approved_time()) { // if using arc_path and finishing current_path, get next path // enough time for turn signal - const bool has_passed_enough_time = (clock_->now() - *last_approved_time_).seconds() > - planner_data_->parameters.turn_signal_search_time; + const bool has_passed_enough_time = + (clock_->now() - *status_.get_last_approved_time()).seconds() > + planner_data_->parameters.turn_signal_search_time; - if (hasFinishedCurrentPath() && has_passed_enough_time && status_.require_increment_) { + if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { if (incrementPathIndex()) { - last_increment_time_ = std::make_unique(clock_->now()); + status_.set_last_increment_time(std::make_shared(clock_->now())); } } } @@ -1164,23 +1191,23 @@ void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() bool GoalPlannerModule::incrementPathIndex() { - if (status_.current_path_idx == status_.pull_over_path->partial_paths.size() - 1) { + if (status_.get_current_path_idx() == status_.get_pull_over_path()->partial_paths.size() - 1) { return false; } - status_.current_path_idx = status_.current_path_idx + 1; + status_.set_current_path_idx(status_.get_current_path_idx() + 1); return true; } PathWithLaneId GoalPlannerModule::getCurrentPath() const { - if (status_.pull_over_path == nullptr) { + if (status_.get_pull_over_path() == nullptr) { return PathWithLaneId{}; } - if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) { + if (status_.get_pull_over_path()->partial_paths.size() <= status_.get_current_path_idx()) { return PathWithLaneId{}; } - return status_.pull_over_path->partial_paths.at(status_.current_path_idx); + return status_.get_pull_over_path()->partial_paths.at(status_.get_current_path_idx()); } bool GoalPlannerModule::isStopped( @@ -1209,6 +1236,7 @@ bool GoalPlannerModule::isStopped( bool GoalPlannerModule::isStopped() { + const std::lock_guard lock(mutex_); return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } @@ -1218,18 +1246,19 @@ bool GoalPlannerModule::isStuck() return false; } + const std::lock_guard lock(mutex_); constexpr double stuck_time = 5.0; if (!isStopped(odometry_buffer_stuck_, stuck_time)) { return false; } // not found safe path - if (!status_.is_safe_static_objects) { + if (!status_.get_is_safe_static_objects()) { return true; } // any path has never been found - if (!status_.pull_over_path) { + if (!status_.get_pull_over_path()) { return false; } @@ -1248,12 +1277,12 @@ bool GoalPlannerModule::hasFinishedCurrentPath() bool GoalPlannerModule::isOnModifiedGoal() const { - if (!modified_goal_pose_) { + if (!status_.get_modified_goal_pose()) { return false; } const Pose current_pose = planner_data_->self_odometry->pose.pose; - return calcDistance2d(current_pose, modified_goal_pose_->goal_pose) < + return calcDistance2d(current_pose, status_.get_modified_goal_pose()->goal_pose) < parameters_->th_arrived_distance; } @@ -1262,9 +1291,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const TurnSignalInfo turn_signal{}; // output const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = status_.pull_over_path->start_pose; - const auto & end_pose = status_.pull_over_path->end_pose; - const auto full_path = status_.pull_over_path->getFullPath(); + const auto & start_pose = status_.get_pull_over_path()->start_pose; + const auto & end_pose = status_.get_pull_over_path()->end_pose; + const auto full_path = status_.get_pull_over_path()->getFullPath(); // calc TurnIndicatorsCommand { @@ -1287,7 +1316,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over turn_signal.desired_start_point = - last_approved_pose_ && status_.has_decided_path ? *last_approved_pose_ : current_pose; + status_.get_last_approved_pose() && status_.get_has_decided_path() + ? *status_.get_last_approved_pose() + : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose; @@ -1344,7 +1375,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - const bool is_separated_path = status_.pull_over_path->partial_paths.size() > 1; + const bool is_separated_path = status_.get_pull_over_path()->partial_paths.size() > 1; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; @@ -1375,10 +1406,10 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) { constexpr double keep_stop_time = 2.0; constexpr double keep_current_idx_buffer_time = 2.0; - if (last_increment_time_) { - const auto time_diff = (clock_->now() - *last_increment_time_).seconds(); + if (status_.get_last_increment_time()) { + const auto time_diff = (clock_->now() - *status_.get_last_increment_time()).seconds(); if (time_diff < keep_stop_time) { - status_.require_increment_ = false; + status_.set_require_increment(false); for (auto & p : path.points) { p.point.longitudinal_velocity_mps = 0.0; } @@ -1386,7 +1417,7 @@ void GoalPlannerModule::keepStoppedWithCurrentPath(PathWithLaneId & path) // require increment only when the time passed is enough // to prevent increment before driving // when the end of the current path is close to the current pose - status_.require_increment_ = true; + status_.set_require_increment(true); } } } @@ -1552,7 +1583,7 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { - const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.lanes); + const lanelet::ConstLanelets lanes = utils::transformToLanelets(status_.get_lanes()); const Pose & start_pose = pull_over_path.start_pose; const Pose & end_pose = pull_over_path.end_pose; @@ -1587,11 +1618,12 @@ bool GoalPlannerModule::isSafePath() const const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(pull_over_path.points); const std::pair terminal_velocity_and_accel = utils::start_goal_planner_common::getPairsTerminalVelocityAndAccel( - status_.pull_over_path->pairs_terminal_velocity_and_accel, status_.current_path_idx); + status_.get_pull_over_path()->pairs_terminal_velocity_and_accel, + status_.get_current_path_idx()); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); + RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.get_current_path_idx()); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly @@ -1612,7 +1644,7 @@ bool GoalPlannerModule::isSafePath() const pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.prev_is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + status_.get_prev_is_safe_dynamic_objects() ? 1.0 : parameters_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1674,29 +1706,31 @@ void GoalPlannerModule::setDebugData() }; if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { // Visualize pull over areas - const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow - : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const double z = refined_goal_pose_.position.z; + const auto color = status_.get_has_decided_path() + ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow + : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green + const double z = status_.get_refined_goal_pose().position.z; add(goal_planner_utils::createPullOverAreaMarkerArray( goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates - add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); + const auto goal_candidates = status_.get_goal_candidates(); + add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color)); } // Visualize path and related pose - if (status_.is_safe_static_objects) { + if (status_.get_is_safe_static_objects()) { add(createPoseMarkerArray( - status_.pull_over_path->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); + status_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); - add( - createPathMarkerArray(status_.pull_over_path->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + status_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + add(createPathMarkerArray( + status_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray(getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < status_.pull_over_path->partial_paths.size(); ++i) { - const auto & partial_path = status_.pull_over_path->partial_paths.at(i); + for (size_t i = 0; i < status_.get_pull_over_path()->partial_paths.size(); ++i) { + const auto & partial_path = status_.get_pull_over_path()->partial_paths.at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -1742,16 +1776,17 @@ void GoalPlannerModule::setDebugData() // Visualize planner type text { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.get_is_safe_static_objects() + ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - marker.pose = modified_goal_pose_ ? modified_goal_pose_->goal_pose - : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(status_.pull_over_path->type); - marker.text += " " + std::to_string(status_.current_path_idx) + "/" + - std::to_string(status_.pull_over_path->partial_paths.size() - 1); + marker.pose = status_.get_modified_goal_pose() ? status_.get_modified_goal_pose()->goal_pose + : planner_data_->self_odometry->pose.pose; + marker.text = magic_enum::enum_name(status_.get_pull_over_path()->type); + marker.text += " " + std::to_string(status_.get_current_path_idx()) + "/" + + std::to_string(status_.get_pull_over_path()->partial_paths.size() - 1); if (isStuck()) { marker.text += " stuck"; } else if (isStopped()) { @@ -1769,7 +1804,7 @@ void GoalPlannerModule::setDebugData() } // Visualize debug poses - const auto & debug_poses = status_.pull_over_path->debug_poses; + const auto & debug_poses = status_.get_pull_over_path()->debug_poses; for (size_t i = 0; i < debug_poses.size(); ++i) { add(createPoseMarkerArray( debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); @@ -1781,7 +1816,8 @@ void GoalPlannerModule::printParkingPositionError() const const auto current_pose = planner_data_->self_odometry->pose.pose; const double real_shoulder_to_map_shoulder = 0.0; - const Pose goal_to_ego = inverseTransformPose(current_pose, modified_goal_pose_->goal_pose); + const Pose goal_to_ego = + inverseTransformPose(current_pose, status_.get_modified_goal_pose()->goal_pose); const double dx = goal_to_ego.position.x; const double dy = goal_to_ego.position.y; const double distance_from_real_shoulder = @@ -1790,7 +1826,7 @@ void GoalPlannerModule::printParkingPositionError() const getLogger(), "current pose to goal, dx:%f dy:%f dyaw:%f from_real_shoulder:%f", dx, dy, tier4_autoware_utils::rad2deg( tf2::getYaw(current_pose.orientation) - - tf2::getYaw(modified_goal_pose_->goal_pose.orientation)), + tf2::getYaw(status_.get_modified_goal_pose()->goal_pose.orientation)), distance_from_real_shoulder); } @@ -1816,10 +1852,10 @@ bool GoalPlannerModule::needPathUpdate(const double path_update_duration) const bool GoalPlannerModule::hasEnoughTimePassedSincePathUpdate(const double duration) const { - if (!last_path_update_time_) { + if (!status_.get_last_path_update_time()) { return true; } - return (clock_->now() - *last_path_update_time_).seconds() > duration; + return (clock_->now() - *status_.get_last_path_update_time()).seconds() > duration; } } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index ff26c7a3654c3..e7fa5e42dc092 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -171,7 +171,7 @@ MarkerArray createNumObjectsToAvoidTextsMarkerArray( } MarkerArray createGoalCandidatesMarkerArray( - GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) + const GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) { GoalCandidates safe_goal_candidates{}; std::copy_if( From eaf9f684dc092ee1681221b7e724081ff1f0833f Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 19 Oct 2023 12:58:42 +0900 Subject: [PATCH 131/206] test(ndt_scan_matcher): add test_ndt_scan_matcher_launch.py (#5347) * Added test_ndt_scan_matcher_launch.py Signed-off-by: Shintaro Sakoda * Added assert and raising exception Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda --- localization/ndt_scan_matcher/CMakeLists.txt | 5 + localization/ndt_scan_matcher/package.xml | 1 + .../src/map_update_module.cpp | 15 +-- .../test/test_ndt_scan_matcher_launch.py | 101 ++++++++++++++++++ 4 files changed, 115 insertions(+), 7 deletions(-) create mode 100644 localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 6f51b5210d853..955234f95ff18 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -41,6 +41,11 @@ link_directories(${PCL_LIBRARY_DIRS}) target_link_libraries(ndt_scan_matcher ${PCL_LIBRARIES}) if(BUILD_TESTING) + add_ros_test( + test/test_ndt_scan_matcher_launch.py + TIMEOUT "30" + ) + find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_tpe test/test_tpe.cpp diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 92c690a708492..986179b9e02e2 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -38,6 +38,7 @@ ament_cmake_cppcheck ament_lint_auto + ros_testing ament_cmake diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp index 9c94467dc94c0..7512ae2e77a21 100644 --- a/localization/ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -55,12 +55,6 @@ MapUpdateModule::MapUpdateModule( pcd_loader_client_ = node->create_client("pcd_loader_service"); - while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { - RCLCPP_INFO( - logger_, - "Waiting for pcd loader service. Check if the enable_differential_load in " - "pointcloud_map_loader is set `true`."); - } double map_update_dt = 1.0; auto period_ns = std::chrono::duration_cast( @@ -117,7 +111,14 @@ void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) request->area.radius = static_cast(dynamic_map_loading_map_radius_); request->cached_ids = ndt_ptr_->getCurrentMapIDs(); - // // send a request to map_loader + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + logger_, + "Waiting for pcd loader service. Check if the enable_differential_load in " + "pointcloud_map_loader is set `true`."); + } + + // send a request to map_loader auto result{pcd_loader_client_->async_send_request( request, [](rclcpp::Client::SharedFuture) {})}; diff --git a/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py new file mode 100644 index 0000000000000..2dc4958c4704f --- /dev/null +++ b/localization/ndt_scan_matcher/test/test_ndt_scan_matcher_launch.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +import pytest +import rclpy +from std_srvs.srv import SetBool + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_ndt_scan_matcher_launch_file = os.path.join( + get_package_share_directory("ndt_scan_matcher"), + "launch", + "ndt_scan_matcher.launch.xml", + ) + ndt_scan_matcher = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_ndt_scan_matcher_launch_file), + ) + + return launch.LaunchDescription( + [ + ndt_scan_matcher, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestNDTScanMatcher(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + + def tearDown(self): + self.test_node.destroy_node() + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_launch(self): + # Trigger ndt_scan_matcher to activate the node + cli_trigger = self.test_node.create_client(SetBool, "/trigger_node") + while not cli_trigger.wait_for_service(timeout_sec=1.0): + continue + + request = SetBool.Request() + request.data = True + future = cli_trigger.call_async(request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is not None: + self.test_node.get_logger().info("Result of bool service: %s" % future.result().message) + self.assertTrue(future.result().success, "ndt_scan_matcher is not activated") + else: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + raise self.failureException("service trigger failed") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) From e67badaf95f53e049b2360f8c3c24025935942f7 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 19 Oct 2023 13:41:20 +0900 Subject: [PATCH 132/206] refactor(ndt_scan_matcher): rename `align_using_monte_carlo` , etc. (#5351) * Renamed align_using_monte_carlo to align_pose Signed-off-by: Shintaro SAKODA * Removed the waste arg ndt_ptr Signed-off-by: Shintaro SAKODA * Fixed log messages Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix --------- Signed-off-by: Shintaro SAKODA Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ndt_scan_matcher_core.hpp | 3 +- .../src/ndt_scan_matcher_core.cpp | 32 ++++++++----------- 2 files changed, 14 insertions(+), 21 deletions(-) diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index a07eb9cd5c8d0..153418e5a8f75 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -92,8 +92,7 @@ class NDTScanMatcher : public rclcpp::Node void callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr); - geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( - const std::shared_ptr & ndt_ptr, + geometry_msgs::msg::PoseWithCovarianceStamped align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); void transform_sensor_measurement( diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 25033c8c83c01..b7f32d2de53ff 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -764,38 +764,33 @@ void NDTScanMatcher::service_ndt_align( map_update_module_->update_map(initial_pose_msg_in_map_frame.pose.pose.position); } + // mutex Map + std::lock_guard lock(ndt_ptr_mtx_); + if (ndt_ptr_->getInputTarget() == nullptr) { res->success = false; - RCLCPP_WARN(get_logger(), "No InputTarget"); + RCLCPP_WARN( + get_logger(), "No InputTarget. Please check the map file and the map_loader service"); return; } if (ndt_ptr_->getInputSource() == nullptr) { res->success = false; - RCLCPP_WARN(get_logger(), "No InputSource"); + RCLCPP_WARN(get_logger(), "No InputSource. Please check the input lidar topic"); return; } - // mutex Map - std::lock_guard lock(ndt_ptr_mtx_); - (*state_ptr_)["state"] = "Aligning"; - res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, initial_pose_msg_in_map_frame); + res->pose_with_covariance = align_pose(initial_pose_msg_in_map_frame); (*state_ptr_)["state"] = "Sleeping"; res->success = true; res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; } -geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_carlo( - const std::shared_ptr & ndt_ptr, +geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) { - if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { - RCLCPP_WARN(get_logger(), "No Map or Sensor PointCloud"); - return geometry_msgs::msg::PoseWithCovarianceStamped(); - } - - output_pose_with_cov_to_log(get_logger(), "align_using_monte_carlo_input", initial_pose_with_cov); + output_pose_with_cov_to_log(get_logger(), "align_pose_input", initial_pose_with_cov); const auto base_rpy = get_rpy(initial_pose_with_cov); const Eigen::Map covariance = { @@ -856,8 +851,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ initial_pose.orientation = tf2::toMsg(tf_quaternion); const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); - ndt_ptr->align(*output_cloud, initial_pose_matrix); - const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); + ndt_ptr_->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); Particle particle( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, @@ -891,7 +886,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ auto sensor_points_in_map_ptr = std::make_shared>(); tier4_autoware_utils::transformPointCloud( - *ndt_ptr->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); + *ndt_ptr_->getInputSource(), *sensor_points_in_map_ptr, ndt_result.pose); publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_in_map_ptr); } @@ -904,8 +899,7 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ result_pose_with_cov_msg.header.frame_id = map_frame_; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; - output_pose_with_cov_to_log( - get_logger(), "align_using_monte_carlo_output", result_pose_with_cov_msg); + output_pose_with_cov_to_log(get_logger(), "align_pose_output", result_pose_with_cov_msg); RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score); return result_pose_with_cov_msg; From 64d80800c500d574bc14c1b65028f2fbfeeebc53 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 19 Oct 2023 14:43:32 +0900 Subject: [PATCH 133/206] refactor(behavior_path_planner): remove unused headers (#5341) * refactor(behavior_path_planner): remove unused headers Signed-off-by: Muhammad Zulfaqar Azmi * style(pre-commit): autofix * additional headers Signed-off-by: Muhammad Zulfaqar * additional headers removed Signed-off-by: Muhammad Zulfaqar --------- Signed-off-by: Muhammad Zulfaqar Azmi Signed-off-by: Muhammad Zulfaqar Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../behavior_path_planner/behavior_path_planner_node.hpp | 7 ------- .../include/behavior_path_planner/parameters.hpp | 1 - .../include/behavior_path_planner/planner_manager.hpp | 3 --- .../dynamic_avoidance/dynamic_avoidance_module.hpp | 5 +++++ .../scene_module/goal_planner/goal_planner_module.hpp | 2 ++ .../scene_module/scene_module_interface.hpp | 1 + .../behavior_path_planner/steering_factor_interface.hpp | 8 +++----- .../utils/avoidance/avoidance_module_data.hpp | 2 +- .../behavior_path_planner/utils/avoidance/utils.hpp | 3 --- .../utils/drivable_area_expansion/footprints.hpp | 6 ------ .../utils/drivable_area_expansion/parameters.hpp | 2 -- .../utils/drivable_area_expansion/path_projection.hpp | 1 - .../geometric_parallel_parking.hpp | 2 -- .../utils/goal_planner/default_fixed_goal_planner.hpp | 1 - .../utils/goal_planner/fixed_goal_planner_base.hpp | 2 -- .../utils/goal_planner/goal_searcher.hpp | 3 --- .../utils/goal_planner/goal_searcher_base.hpp | 2 -- .../utils/goal_planner/pull_over_planner_base.hpp | 1 - .../utils/lane_change/lane_change_module_data.hpp | 8 -------- .../behavior_path_planner/utils/lane_change/utils.hpp | 5 ----- .../utils/lane_following/module_data.hpp | 3 --- .../utils/path_safety_checker/objects_filtering.hpp | 1 - .../utils/path_safety_checker/safety_check.hpp | 4 ---- .../utils/path_shifter/path_shifter.hpp | 3 --- .../behavior_path_planner/utils/side_shift/util.hpp | 3 --- .../start_goal_planner_common/common_module_data.hpp | 1 - .../utils/start_planner/freespace_pull_out.hpp | 1 - .../utils/start_planner/pull_out_planner_base.hpp | 6 ++---- .../behavior_path_planner/utils/start_planner/util.hpp | 2 -- .../include/behavior_path_planner/utils/utils.hpp | 8 -------- .../src/behavior_path_planner_node.cpp | 8 ++++++-- .../src/marker_utils/avoidance/debug.cpp | 3 --- .../src/marker_utils/lane_change/debug.cpp | 2 -- .../behavior_path_planner/src/marker_utils/utils.cpp | 1 - planning/behavior_path_planner/src/planner_manager.cpp | 1 - .../src/scene_module/avoidance/avoidance_module.cpp | 5 ----- .../dynamic_avoidance/dynamic_avoidance_module.cpp | 3 +-- .../scene_module/goal_planner/goal_planner_module.cpp | 3 +-- .../lane_change/avoidance_by_lane_change.cpp | 1 - .../src/scene_module/lane_change/external_request.cpp | 9 --------- .../src/scene_module/lane_change/interface.cpp | 2 -- .../src/scene_module/lane_change/manager.cpp | 1 - .../src/scene_module/lane_change/normal.cpp | 2 +- .../src/scene_module/side_shift/side_shift_module.cpp | 1 - .../scene_module/start_planner/start_planner_module.cpp | 2 -- .../behavior_path_planner/src/utils/avoidance/utils.cpp | 1 - .../geometric_parallel_parking.cpp | 3 --- .../utils/goal_planner/default_fixed_goal_planner.cpp | 1 - .../src/utils/goal_planner/geometric_pull_over.cpp | 2 -- .../src/utils/goal_planner/goal_searcher.cpp | 2 -- .../src/utils/goal_planner/shift_pull_over.cpp | 2 -- .../src/utils/goal_planner/util.cpp | 4 ---- .../src/utils/lane_change/utils.cpp | 4 ++-- .../src/utils/path_safety_checker/objects_filtering.cpp | 1 + .../src/utils/path_safety_checker/safety_check.cpp | 6 +++--- .../src/utils/path_shifter/path_shifter.cpp | 2 -- planning/behavior_path_planner/src/utils/path_utils.cpp | 2 -- planning/behavior_path_planner/src/utils/utils.cpp | 5 ++--- planning/behavior_path_planner/test/input.hpp | 7 ++++++- 59 files changed, 37 insertions(+), 146 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index e8ad6c7e2f987..420a5a8aa6ee5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -17,13 +17,7 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/scene_module/avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/side_shift/manager.hpp" -#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" @@ -66,7 +60,6 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::PoseWithUuidStamped; -using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index e3a3784c5e928..40df1f5157c71 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -18,7 +18,6 @@ #include #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 4b9b0bff4be63..c0437aa69764a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -18,20 +18,17 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include -#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include #include #include #include -#include #include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index ab8e3cfd70dff..6c522c79a8774 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -18,8 +18,10 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include +#include #include +#include #include #include #include @@ -35,6 +37,9 @@ namespace behavior_path_planner { +using autoware_auto_perception_msgs::msg::PredictedPath; +using tier4_autoware_utils::Polygon2d; + struct MinMaxValue { double min_value{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 632872471ce29..251b72ea6eadb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -32,6 +32,7 @@ #include #include #include +#include #include #include @@ -65,6 +66,7 @@ using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; +using tier4_autoware_utils::Polygon2d; enum class PathType { NONE = 0, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index 68faa3806f614..7f24683e5680c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -16,6 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ #include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/utils.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp index 8afe0576ec1aa..3a334cf67edbf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp @@ -15,15 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ #define BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ -#include "rclcpp/rclcpp.hpp" +#include -#include "autoware_adapi_v1_msgs/msg/steering_factor_array.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "unique_identifier_msgs/msg/uuid.hpp" +#include +#include #include #include -#include namespace steering_factor_interface { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index d04b35a3b185b..74464be0fa5ea 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 8cb09cd47b444..0237fb458ea0b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -18,11 +18,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 418a9a5a68572..9591ac0582e04 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -20,7 +20,6 @@ #include -#include "autoware_auto_planning_msgs/msg/detail/path_point__struct.hpp" #include #include #include @@ -31,11 +30,6 @@ #include #include -#include -#include -#include -#include - namespace drivable_area_expansion { /// @brief translate a polygon by some (x,y) vector diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp index e7275960b0888..2f9604223a248 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp @@ -15,8 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" - #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp index 93afaad825582..3f5327acbca68 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp @@ -21,7 +21,6 @@ #include -#include #include namespace drivable_area_expansion diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index c2d36fdd6e0d7..f4ab10db61b9c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -30,9 +30,7 @@ #include -#include #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp index 40c339e507683..9cede67fff6e5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -20,7 +20,6 @@ #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp index 3bc0e960fe468..f587051442dcd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp @@ -16,13 +16,11 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include #include #include -#include using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 9d957ae9767c9..2ed58b9678e70 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -18,14 +18,11 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" - #include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::LinearRing2d; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index 7364ea91339e4..ab319111f6da6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -16,12 +16,10 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 20788e53309ec..54bba6aee2fc2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 8b9ef52394cdd..c664ae3e15aef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -16,17 +16,9 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "lanelet2_core/geometry/Lanelet.h" - -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" #include -#include -#include -#include -#include #include namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index cd8f293b2e610..ac712fa2f0fa5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -15,12 +15,10 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/utils.hpp" #include @@ -32,10 +30,7 @@ #include -#include #include -#include -#include #include namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp index 93e4542568745..98d52b79e2edf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp @@ -15,9 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_FOLLOWING__MODULE_DATA_HPP_ -#include -#include - namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 15b15bac40b51..67588ed0a67b6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -27,7 +27,6 @@ #include #include -#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 64f5c9e5fc3e0..8409116236169 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -35,7 +34,6 @@ #include #endif -#include #include namespace behavior_path_planner::utils::path_safety_checker @@ -52,8 +50,6 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; -namespace bg = boost::geometry; - bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp index 4b1115a049ab0..91ed95b14b905 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp @@ -15,8 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#include "behavior_path_planner/parameters.hpp" - #include #include @@ -24,7 +22,6 @@ #include -#include #include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp index 65d79a2a4977b..6cdfd84d79075 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/util.hpp @@ -20,9 +20,6 @@ #include #include -#include -#include - namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp index 55b8bdf777692..7a973a3bd699f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index 2b2de183b2dac..d78df16f89a80 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -24,7 +24,6 @@ #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index cb662efd767bf..4c9271d57127d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" @@ -27,14 +26,13 @@ #include #include -#include +namespace behavior_path_planner +{ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; -namespace behavior_path_planner -{ enum class PlannerType { NONE = 0, SHIFT = 1, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index f2dba684de991..e7f51b2e86d5b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include @@ -32,7 +31,6 @@ #include #include -#include namespace behavior_path_planner::start_planner_utils { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 3e03a09d3adf8..f418c9c7300f7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -16,14 +16,8 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ #include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/lane_following/module_data.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -51,8 +45,6 @@ #include #include #include -#include -#include #include namespace behavior_path_planner::utils diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3fb9060835698..52adfc72ce761 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -15,8 +15,13 @@ #include "behavior_path_planner/behavior_path_planner_node.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner/scene_module/avoidance/manager.hpp" +#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" +#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" #include "behavior_path_planner/scene_module/lane_change/interface.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" +#include "behavior_path_planner/scene_module/lane_change/manager.hpp" +#include "behavior_path_planner/scene_module/side_shift/manager.hpp" +#include "behavior_path_planner/scene_module/start_planner/manager.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include @@ -26,7 +31,6 @@ #include #include -#include #include namespace diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index 14a7d5be6a58a..ce9cedc816a19 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -14,9 +14,7 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include @@ -29,7 +27,6 @@ namespace marker_utils::avoidance_marker { -using behavior_path_planner::AvoidLine; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcOffsetPose; diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index a1b77b1802999..a7604124ebe43 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -25,8 +25,6 @@ #include #include -#include -#include #include #include diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index 03ac135ec4bc1..031926f2d9d1d 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -17,7 +17,6 @@ #include "behavior_path_planner/marker_utils/colors.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index d0e55ffa574fd..4c3d7cff0c24c 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -14,7 +14,6 @@ #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index b4b0a871eec62..84064a1c38bcb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -52,17 +52,12 @@ namespace behavior_path_planner { -using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; -using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcInterpolatedPose; using tier4_autoware_utils::calcLateralDeviation; -using tier4_autoware_utils::calcLongitudinalDeviation; -using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; using tier4_autoware_utils::toHexString; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 24c074bcb629b..d8434593d2214 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -14,8 +14,8 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include @@ -30,7 +30,6 @@ #include #include #include -#include #include #include namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index dc1ac1466397d..6253097b1692d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -17,11 +17,11 @@ #include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -38,7 +38,6 @@ #include using behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance; -using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index 1f305e3988828..daba02e1cee89 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -17,7 +17,6 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp index a74a51b7e2cdb..3a304a9b5bb53 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/external_request.cpp @@ -14,18 +14,9 @@ #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" - #include -#include -#include #include -#include -#include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 0a0cc3436bce7..22c8ac022ad1a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -19,12 +19,10 @@ #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include #include -#include #include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index c6995ada1bfa6..78e05c940a814 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -21,7 +21,6 @@ #include #include -#include #include namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index de4afc9260da0..de120592e7505 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -15,9 +15,9 @@ #include "behavior_path_planner/scene_module/lane_change/normal.hpp" #include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 4e9680cee4d59..2232c6d750d55 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -32,7 +32,6 @@ using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index c828a747856d8..ed14292747e68 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -20,7 +20,6 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include @@ -39,7 +38,6 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPoint; namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 8fb30a28f1272..a9f40dad86ae4 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -18,7 +18,6 @@ #include "behavior_path_planner/utils/avoidance/utils.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 3d315e7213f96..d5bf67f754ab7 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -17,7 +17,6 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -37,8 +36,6 @@ #include -#include -#include #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 7efdbdf1552d5..4c9832c374c63 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -21,7 +21,6 @@ #include #include -#include namespace behavior_path_planner { diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index a80f7892b0bef..017ecb218bea3 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -15,8 +15,6 @@ #include "behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 12044980ebd81..1e6dc1776359b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -20,7 +20,6 @@ #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -37,7 +36,6 @@ using lane_departure_checker::LaneDepartureChecker; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::inverseTransformPose; // Sort with smaller longitudinal distances taking precedence over smaller lateral distances. struct SortByLongitudinalDistance diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index e1bdaf977dad8..60cc2d19c5f35 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -16,8 +16,6 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index e7fa5e42dc092..1cd985d566f73 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -14,9 +14,6 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" - #include #include #include @@ -29,7 +26,6 @@ #include #include -#include #include #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 6d25cb8751a8e..d5867c40824d2 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -14,13 +14,14 @@ #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -39,7 +40,6 @@ #include #include -#include #include #include diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index dfc9f76b25e33..a7c604985de3a 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 08ebd01ba5d05..8ebc144a34584 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -14,11 +14,8 @@ #include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" #include "interpolation/linear_interpolation.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "object_recognition_utils/predicted_path_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -27,6 +24,9 @@ namespace behavior_path_planner::utils::path_safety_checker { + +namespace bg = boost::geometry; + void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { Point2d point; diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 21d4ff666e28a..df2d82b1072d4 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -15,13 +15,11 @@ #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" #include #include #include -#include #include #include #include diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 1ac63f4a4be87..3ac3c09ba0d97 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -15,7 +15,6 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include @@ -23,7 +22,6 @@ #include #include -// #include #include #include diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index cb0486b6cd01b..6927327e24ef4 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -15,7 +15,9 @@ #include "behavior_path_planner/utils/utils.hpp" #include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "object_recognition_utils/predicted_path_utils.hpp" #include #include @@ -24,9 +26,6 @@ #include #include -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" - #include #include diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/test/input.hpp index de167bc8b4bcb..ededb32f78be0 100644 --- a/planning/behavior_path_planner/test/input.hpp +++ b/planning/behavior_path_planner/test/input.hpp @@ -16,9 +16,11 @@ #define INPUT_HPP_ #endif // INPUT_HPP_ -#include "behavior_path_planner/utils/utils.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" #include @@ -26,6 +28,9 @@ namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); From 45b05031b3ed3fa83033b2e7c3763115b121d589 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Thu, 19 Oct 2023 15:03:22 +0900 Subject: [PATCH 134/206] perf(pointcloud_preprocessor): move print out of hot loop (#5283) * perf(pointcloud_preprocessor): move print out of hot loop Signed-off-by: Vincent Richard * style(pre-commit): autofix --------- Signed-off-by: Vincent Richard Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/crop_box_filter/crop_box_filter_nodelet.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp index 0cae2c90c2f16..20d1f5c8b3d6d 100644 --- a/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_nodelet.cpp @@ -119,7 +119,8 @@ void CropBoxFilterComponent::faster_filter( stop_watch_ptr_->toc("processing_time", true); if (indices) { - RCLCPP_WARN(get_logger(), "Indices are not supported and will be ignored"); + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "Indices are not supported and will be ignored"); } int x_offset = input->fields[pcl::getFieldIndex(*input, "x")].offset; @@ -129,6 +130,8 @@ void CropBoxFilterComponent::faster_filter( output.data.resize(input->data.size()); size_t output_size = 0; + int skipped_count = 0; + for (size_t global_offset = 0; global_offset + input->point_step <= input->data.size(); global_offset += input->point_step) { Eigen::Vector4f point; @@ -138,7 +141,7 @@ void CropBoxFilterComponent::faster_filter( point[3] = 1; if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) { - RCLCPP_WARN(this->get_logger(), "Ignoring point containing NaN values"); + skipped_count++; continue; } @@ -162,6 +165,12 @@ void CropBoxFilterComponent::faster_filter( } } + if (skipped_count > 0) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 1000, "%d points contained NaN values and have been ignored", + skipped_count); + } + output.data.resize(output_size); // Note that tf_input_orig_frame_ is the input frame, while tf_input_frame_ is the frame of the From 48568b737e07d580c66e2819aa6c15bd94a10a42 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 19 Oct 2023 16:05:03 +0900 Subject: [PATCH 135/206] fix(ndt_scan_matcher): fix `validate_num_iteration` (#5354) Fixed validate_num_iteration Signed-off-by: Shintaro SAKODA --- .../ndt_scan_matcher/src/ndt_scan_matcher_core.cpp | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index b7f32d2de53ff..8e583fd3a666b 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -448,20 +448,8 @@ void NDTScanMatcher::callback_sensor_points( } // perform several validations - /***************************************************************************** - The reason the add 2 to the ndt_ptr_->getMaximumIterations() is that there are bugs in - implementation of ndt. - 1. gradient descent method ends when the iteration is greater than max_iteration if it does not - converge (be careful it's 'greater than' instead of 'greater equal than'.) - https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L212 - 2. iterate iteration count when end of gradient descent function. - https://github.com/tier4/autoware.iv/blob/2323e5baa0b680d43a9219f5fb3b7a11dd9edc82/localization/pose_estimator/ndt_scan_matcher/ndt_omp/include/ndt_omp/ndt_omp_impl.hpp#L217 - - These bugs are now resolved in original pcl implementation. - https://github.com/PointCloudLibrary/pcl/blob/424c1c6a0ca97d94ca63e5daff4b183a4db8aae4/registration/include/pcl/registration/impl/ndt.hpp#L73-L180 - *****************************************************************************/ bool is_ok_iteration_num = - validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations() + 2); + validate_num_iteration(ndt_result.iteration_num, ndt_ptr_->getMaximumIterations()); bool is_local_optimal_solution_oscillation = false; if (!is_ok_iteration_num) { is_local_optimal_solution_oscillation = validate_local_optimal_solution_oscillation( From 0d0a22c175a333a487f434c01da2ebb15621db24 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 19 Oct 2023 16:17:36 +0900 Subject: [PATCH 136/206] feat(planner_manager): limit iteration number by parameter (#5352) Signed-off-by: satoshi-ota --- .../config/behavior_path_planner.param.yaml | 1 + .../behavior_path_planner/parameters.hpp | 1 + .../behavior_path_planner/planner_manager.hpp | 4 +++- .../src/behavior_path_planner_node.cpp | 3 ++- .../src/planner_manager.cpp | 17 ++++++++++++++--- 5 files changed, 21 insertions(+), 5 deletions(-) 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 95c60612bfdb7..d333cbfd778cb 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: verbose: false + max_iteration_num: 100 planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 40df1f5157c71..a4e6fc18ab050 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -76,6 +76,7 @@ struct LateralAccelerationMap struct BehaviorPathPlannerParameters { bool verbose; + size_t max_iteration_num{100}; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index c0437aa69764a..1d1ad56342f4d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -94,7 +94,7 @@ struct SceneModuleStatus class PlannerManager { public: - PlannerManager(rclcpp::Node & node, const bool verbose); + PlannerManager(rclcpp::Node & node, const size_t max_iteration_num, const bool verbose); /** * @brief run all candidate and approved modules. @@ -443,6 +443,8 @@ class PlannerManager mutable std::shared_ptr debug_msg_ptr_; + size_t max_iteration_num_{100}; + bool verbose_{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 52adfc72ce761..0777822650dfb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -136,7 +136,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const std::lock_guard lock(mutex_manager_); // for planner_manager_ const auto & p = planner_data_->parameters; - planner_manager_ = std::make_shared(*this, p.verbose); + planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); const auto register_and_create_publisher = [&](const auto & manager, const bool create_publishers) { @@ -276,6 +276,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() BehaviorPathPlannerParameters p{}; p.verbose = declare_parameter("verbose"); + p.max_iteration_num = declare_parameter("max_iteration_num"); const auto get_scene_module_manager_param = [&](std::string && ns) { ModuleConfigParameters config; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 4c3d7cff0c24c..442049654d64e 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -28,9 +28,11 @@ namespace behavior_path_planner { -PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) +PlannerManager::PlannerManager( + rclcpp::Node & node, const size_t max_iteration_num, const bool verbose) : logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), + max_iteration_num_{max_iteration_num}, verbose_{verbose} { processing_time_.emplace("total_time", 0.0); @@ -81,7 +83,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da return output; } - while (rclcpp::ok()) { + for (size_t itr_num = 0;; ++itr_num) { /** * STEP1: get approved modules' output */ @@ -127,8 +129,17 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da addApprovedModule(highest_priority_module); clearCandidateModules(); debug_info_.emplace_back(highest_priority_module, Action::ADD, "To Approval"); + + if (itr_num >= max_iteration_num_) { + RCLCPP_WARN_THROTTLE( + logger_, clock_, 1000, "Reach iteration limit (max: %ld). Output current result.", + max_iteration_num_); + processing_time_.at("total_time") = stop_watch_.toc("total_time", true); + return candidate_modules_output; + } } - return BehaviorModuleOutput{}; + + return BehaviorModuleOutput{}; // something wrong. }(); std::for_each( From c75d581016c559e7bd8a6bf3e63b173c13e30d63 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Thu, 19 Oct 2023 17:18:14 +0900 Subject: [PATCH 137/206] feat(tier4_perception_launch): add parameter to control detection_by_tracker on/off (#5313) * add parameter to control detection_by_tracker on/off Signed-off-by: yoshiri * style(pre-commit): autofix * Update launch/tier4_perception_launch/launch/perception.launch.xml Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --------- Signed-off-by: yoshiri Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- ...ra_lidar_fusion_based_detection.launch.xml | 19 ++++++++++++----- ...ar_radar_fusion_based_detection.launch.xml | 21 +++++++++++++------ .../lidar_based_detection.launch.xml | 19 ++++++++++++----- .../launch/perception.launch.xml | 3 +++ 4 files changed, 46 insertions(+), 16 deletions(-) diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 67e123ea5b018..59fe3f13f1231 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -192,7 +192,7 @@ - + @@ -329,19 +329,24 @@ + + + + - + - + + @@ -356,16 +361,20 @@ + + - + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 07640495a5e19..11deaad5d06cc 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -217,7 +217,7 @@ - + @@ -367,21 +367,26 @@ + + + + - + - + + - + @@ -394,16 +399,20 @@ + + - + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index c844db39f4e9d..822c251ddad33 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -58,7 +58,7 @@ - + @@ -128,18 +128,23 @@ + + + + - + - + + @@ -154,16 +159,20 @@ + + - + + + - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 63ea0cc7f0f3d..347606d91759f 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -96,6 +96,9 @@ + + + Date: Thu, 19 Oct 2023 23:12:44 +0900 Subject: [PATCH 138/206] feat(map_based_prediction): remove crossing fence path (#5356) * remove crossing fence path Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../map_based_prediction_node.hpp | 8 +++ .../src/map_based_prediction_node.cpp | 49 +++++++++++++++++++ 2 files changed, 57 insertions(+) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 3d01ab96e9b62..c4c7ff4a8e942 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -165,6 +165,14 @@ class MapBasedPredictionNode : public rclcpp::Node void mapCallback(const HADMapBin::ConstSharedPtr msg); void objectsCallback(const TrackedObjects::ConstSharedPtr in_objects); + bool doesPathCrossAnyFence(const PredictedPath & predicted_path); + bool doesPathCrossFence( + const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line); + lanelet::BasicLineString2d convertToFenceLine(const lanelet::ConstLineString3d & fence); + bool isIntersecting( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, + const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4); + PredictedObjectKinematics convertToPredictedKinematics( const TrackedObjectKinematics & tracked_object); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 745bccf6dbabe..6604a9dc1539a 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -976,6 +976,46 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt pub_calculation_time_->publish(calculation_time_msg); } +bool MapBasedPredictionNode::doesPathCrossAnyFence(const PredictedPath & predicted_path) +{ + const lanelet::ConstLineStrings3d & all_fences = + lanelet::utils::query::getAllFences(lanelet_map_ptr_); + for (const auto & fence_line : all_fences) { + if (doesPathCrossFence(predicted_path, fence_line)) { + return true; + } + } + return false; +} + +bool MapBasedPredictionNode::doesPathCrossFence( + const PredictedPath & predicted_path, const lanelet::ConstLineString3d & fence_line) +{ + // check whether the predicted path cross with fence + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + for (size_t j = 0; j < fence_line.size() - 1; ++j) { + if (isIntersecting( + predicted_path.path[i].position, predicted_path.path[i + 1].position, fence_line[j], + fence_line[j + 1])) { + return true; + } + } + } + return false; +} + +bool MapBasedPredictionNode::isIntersecting( + const geometry_msgs::msg::Point & point1, const geometry_msgs::msg::Point & point2, + const lanelet::ConstPoint3d & point3, const lanelet::ConstPoint3d & point4) +{ + const auto p1 = tier4_autoware_utils::createPoint(point1.x, point1.y, 0.0); + const auto p2 = tier4_autoware_utils::createPoint(point2.x, point2.y, 0.0); + const auto p3 = tier4_autoware_utils::createPoint(point3.x(), point3.y(), 0.0); + const auto p4 = tier4_autoware_utils::createPoint(point4.x(), point4.y(), 0.0); + const auto intersection = tier4_autoware_utils::intersect(p1, p2, p3, p4); + return intersection.has_value(); +} + PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( const TrackedObject & object) { @@ -995,6 +1035,7 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } + // If the object is in the crosswalk, generate path to the crosswalk edge if (crossing_crosswalk) { const auto edge_points = getCrosswalkEdgePoints(crossing_crosswalk.get()); @@ -1018,6 +1059,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( predicted_object.kinematics.predicted_paths.push_back(predicted_path); } + // If the object is not crossing the crosswalk, in the road lanelets, try to find the closest + // crosswalk and generate path to the crosswalk edge } else if (withinRoadLanelet(object, lanelet_map_ptr_)) { lanelet::ConstLanelet closest_crosswalk{}; const auto & obj_pose = object.kinematics.pose_with_covariance.pose; @@ -1048,6 +1091,8 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( } } + // If the object is not crossing the crosswalk, not in the road lanelets, try to find the edge + // points for all crosswalks and generate path to the crosswalk edge } else { for (const auto & crosswalk : crosswalks_) { const auto edge_points = getCrosswalkEdgePoints(crosswalk); @@ -1080,6 +1125,10 @@ PredictedObject MapBasedPredictionNode::getPredictedObjectAsCrosswalkUser( if (predicted_path.path.empty()) { continue; } + // If the predicted path to the crosswalk is crossing the fence, don't use it + if (doesPathCrossAnyFence(predicted_path)) { + continue; + } predicted_object.kinematics.predicted_paths.push_back(predicted_path); } From 55e0deabf8f9f4fab0aa4c8b50f6db2d7a706610 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 20 Oct 2023 09:33:57 +0900 Subject: [PATCH 139/206] fix(route_handler): fix getting next lane logic to prevent from unexpected path cut (#5355) Signed-off-by: satoshi-ota --- planning/route_handler/src/route_handler.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ba9c46f157f2b..cf309a294d81f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -855,12 +855,11 @@ bool RouteHandler::getNextLaneletWithinRoute( return false; } - lanelet::ConstLanelet start_lanelet; - const bool flag_check = getClosestLaneletWithinRoute(route_ptr_->start_pose, &start_lanelet); + const auto start_lane_id = route_ptr_->segments.front().preferred_primitive.id; const auto following_lanelets = routing_graph_ptr_->following(lanelet); for (const auto & llt : following_lanelets) { - if (!(flag_check && start_lanelet.id() == llt.id()) && exists(route_lanelets_, llt)) { + if (start_lane_id != llt.id() && exists(route_lanelets_, llt)) { *next_lanelet = llt; return true; } From d36be5529cc66b27be0edb314b7647b210663b37 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Fri, 20 Oct 2023 11:14:21 +0900 Subject: [PATCH 140/206] feat(map_based_prediction): enable to control lateral path convergence speed by setting control time horizon (#5285) * enable to control lateral path convergence speed by setting control time horizon Signed-off-by: yoshiri * update readme Signed-off-by: yoshiri * add comment in generate path function Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- perception/map_based_prediction/README.md | 16 ++++++++++++++ .../config/map_based_prediction.param.yaml | 1 + .../map_based_prediction_node.hpp | 1 + .../map_based_prediction/path_generator.hpp | 5 +++-- .../src/map_based_prediction_node.cpp | 5 ++++- .../src/path_generator.cpp | 22 +++++++++++++------ 6 files changed, 40 insertions(+), 10 deletions(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 7631b47772862..6d3d7f5e035a9 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -109,6 +109,21 @@ For the additional information, here we show how we calculate lateral velocity. Currently, we use the upper method with a low-pass filter to calculate lateral velocity. +### Path generation + +Path generation is generated on the frenet frame. The path is generated by the following steps: + +1. Get the frenet frame of the reference path. +2. Generate the frenet frame of the current position of the object and the finite position of the object. +3. Optimize the path in each longitudinal and lateral coordinate of the frenet frame. (Use the quintic polynomial to fit start and end conditions.) +4. Convert the path to the global coordinate. + +See paper [2] for more details. + +#### Tuning lateral path shape + +`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) + ### Path prediction for crosswalk users This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: @@ -155,6 +170,7 @@ If the target object is inside the road or crosswalk, this module outputs one or | ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | | `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | | `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `lateral_control_time_horizon` | [s] | double | time duration for predicted path will reach the reference path (mostly center of the lane) | | `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | | `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | | `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 31fae9c811092..260ae45da2e05 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -2,6 +2,7 @@ ros__parameters: enable_delay_compensation: true prediction_time_horizon: 10.0 #[s] + lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] min_crosswalk_user_velocity: 1.39 #[m/s] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index c4c7ff4a8e942..01f39057aef36 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -135,6 +135,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; + double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; double min_velocity_for_map_based_prediction_; diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 7bb1542557d2c..4da4f62be2ede 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -81,8 +81,8 @@ class PathGenerator { public: PathGenerator( - const double time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity); + const double time_horizon, const double lateral_time_horizon, + const double sampling_time_interval, const double min_crosswalk_user_velocity); PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); @@ -102,6 +102,7 @@ class PathGenerator private: // Parameters double time_horizon_; + double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 6604a9dc1539a..08fc06330b1d8 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -694,6 +694,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ { enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + lateral_control_time_horizon_ = + declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); min_velocity_for_map_based_prediction_ = declare_parameter("min_velocity_for_map_based_prediction"); @@ -740,7 +742,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); path_generator_ = std::make_shared( - prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, + min_crosswalk_user_velocity_); sub_objects_ = this->create_subscription( "~/input/objects", 1, diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 547132410badf..5cb7e186b7cc4 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -23,9 +23,10 @@ namespace map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double sampling_time_interval, + const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, const double min_crosswalk_user_velocity) : time_horizon_(time_horizon), + lateral_time_horizon_(lateral_time_horizon), sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { @@ -213,18 +214,25 @@ FrenetPath PathGenerator::generateFrenetPath( { FrenetPath path; const double duration = time_horizon_; + const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory - const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration); + const Eigen::Vector3d lat_coeff = + calcLatCoefficients(current_point, target_point, lateral_duration); const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration); path.reserve(static_cast(duration / sampling_time_interval_)); for (double t = 0.0; t <= duration; t += sampling_time_interval_) { - const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) + - lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) + - lat_coeff(2) * std::pow(t, 5); - const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) + - lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4); + const double current_acc = + 0.0; // Currently we assume the object is traveling at a constant speed + const double d_next_ = current_point.d + current_point.d_vel * t + + current_acc * 2.0 * std::pow(t, 2) + lat_coeff(0) * std::pow(t, 3) + + lat_coeff(1) * std::pow(t, 4) + lat_coeff(2) * std::pow(t, 5); + // t > lateral_duration: 0.0, else d_next_ + const double d_next = t > lateral_duration ? 0.0 : d_next_; + const double s_next = current_point.s + current_point.s_vel * t + + 2.0 * current_acc * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) + + lon_coeff(1) * std::pow(t, 4); if (s_next > max_length) { break; } From 915d47dbb1bc01f27b33a9e4577fb14e83a609e6 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Fri, 20 Oct 2023 16:42:06 +0900 Subject: [PATCH 141/206] fix(obstacle_stop_planner, dynamic avoidance): fix coordinate transformation bug (#5248) fix a known bug https://github.com/autowarefoundation/autoware.universe/pull/5180 Signed-off-by: Yuki Takagi --- .../dynamic_avoidance_module.cpp | 21 ++++++++----------- .../src/adaptive_cruise_control.cpp | 19 +++++++++++------ 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index d8434593d2214..2c5e78a267d5a 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -32,6 +32,7 @@ #include #include #include + namespace behavior_path_planner { namespace @@ -105,21 +106,17 @@ std::pair projectObstacleVelocityToTrajectory( const std::vector & path_points, const PredictedObject & object) { const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose; - const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); - - const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); - const double obj_yaw = tf2::getYaw(obj_pose.orientation); - const double obj_vel_yaw = - obj_yaw + std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); + const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position); const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation); - const double diff_yaw = tier4_autoware_utils::normalizeRadian(obj_vel_yaw - path_yaw); - return std::make_pair(obj_vel_norm * std::cos(diff_yaw), obj_vel_norm * std::sin(diff_yaw)); + const Eigen::Rotation2Dd R_ego_to_obstacle(obj_yaw - path_yaw); + const Eigen::Vector2d obstacle_velocity( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], projected_velocity[1]); } double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index b33342b9dea4f..c0f57489078d6 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -427,9 +427,12 @@ bool AdaptiveCruiseController::estimatePointVelocityFromObject( obj_vel_norm = std::hypot( obj.kinematics.initial_twist_with_covariance.twist.linear.x, obj.kinematics.initial_twist_with_covariance.twist.linear.y); - obj_vel_yaw = std::atan2( - obj.kinematics.initial_twist_with_covariance.twist.linear.y, - obj.kinematics.initial_twist_with_covariance.twist.linear.x); + + const double obj_yaw = + tf2::getYaw(obj.kinematics.initial_pose_with_covariance.pose.orientation); + obj_vel_yaw = obj_yaw + std::atan2( + obj.kinematics.initial_twist_with_covariance.twist.linear.y, + obj.kinematics.initial_twist_with_covariance.twist.linear.x); get_obj = true; break; } @@ -451,9 +454,13 @@ void AdaptiveCruiseController::calculateProjectedVelocityFromObject( double obj_vel_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); - double obj_vel_yaw = std::atan2( - object.kinematics.initial_twist_with_covariance.twist.linear.y, - object.kinematics.initial_twist_with_covariance.twist.linear.x); + + const double obj_yaw = + tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation); + const double obj_vel_yaw = + obj_yaw + std::atan2( + object.kinematics.initial_twist_with_covariance.twist.linear.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x); *velocity = obj_vel_norm * std::cos(tier4_autoware_utils::normalizeRadian(obj_vel_yaw - traj_yaw)); From e519c4b5f5e2900d1a62fc0182ade9b2df125fab Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Fri, 20 Oct 2023 16:51:31 +0900 Subject: [PATCH 142/206] chore(behavior_velocity_crosswalk_module): add maintainer (#5363) * add maintainer Signed-off-by: kyoichi-sugahara * style(pre-commit): autofix --------- Signed-off-by: kyoichi-sugahara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/behavior_velocity_crosswalk_module/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 66f326ed799af..3830aa9edddff 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -9,6 +9,7 @@ Tomoya Kimura Shumpei Wakabayashi Takayuki Murooka + Kyoichi Sugahara Apache License 2.0 From 33bc810a591fefa9b52339d03ac54ff92472bc28 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 20 Oct 2023 19:04:52 +0900 Subject: [PATCH 143/206] feat(intersection): timeout static occlusion with traffic light (#5353) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 1 + .../src/debug.cpp | 14 +- .../src/manager.cpp | 2 + .../src/scene_intersection.cpp | 152 +++++++++++++----- .../src/scene_intersection.hpp | 23 ++- .../src/util.cpp | 2 +- .../src/util_type.hpp | 4 +- 7 files changed, 146 insertions(+), 52 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 1a5143678ddce..be8e822b31d5c 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -76,6 +76,7 @@ maximum_peeking_distance: 6.0 # [m] attention_lane_crop_curvature_threshold: 0.25 attention_lane_curvature_calculation_ds: 0.5 + static_occlusion_with_traffic_light_timeout: 3.5 enable_rtc: intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 3d5874c3c89e8..f472c4bf51e31 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -188,14 +188,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array); } - if (debug_data_.intersection_area) { - appendMarkerArray( - debug::createPolygonMarkerArray( - debug_data_.intersection_area.value(), "intersection_area", lane_id_, now, 0.3, 0.0, 0.0, - 0.0, 1.0, 0.0), - &debug_marker_array); - } - if (debug_data_.stuck_vehicle_detect_area) { appendMarkerArray( debug::createPolygonMarkerArray( @@ -309,6 +301,12 @@ motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() if (debug_data_.occlusion_stop_wall_pose) { wall.style = motion_utils::VirtualWallType::stop; wall.text = "intersection_occlusion"; + if (debug_data_.static_occlusion_with_traffic_light_timeout) { + std::stringstream timeout; + timeout << std::setprecision(2) + << debug_data_.static_occlusion_with_traffic_light_timeout.value(); + wall.text += "(" + timeout.str() + ")"; + } wall.ns = "intersection_occlusion" + std::to_string(module_id_) + "_"; wall.pose = debug_data_.occlusion_stop_wall_pose.value(); virtual_walls.push_back(wall); diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 22fa57f20e79f..0b4131b55ed90 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -172,6 +172,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); ip.occlusion.attention_lane_curvature_calculation_ds = getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( + node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index b37b70f290ff6..1bbff9ecaf048 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -129,6 +129,11 @@ IntersectionModule::IntersectionModule( planner_param_.occlusion.before_creep_stop_time); temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP); } + { + static_occlusion_timeout_state_machine_.setMarginTime( + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout); + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); @@ -589,6 +594,8 @@ void reactRTCApprovalByDecisionResult( planning_utils::setVelocityFromIndex(occlusion_peeking_stop_line, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_peeking_stop_line, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; @@ -648,6 +655,8 @@ void reactRTCApprovalByDecisionResult( planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + debug_data->static_occlusion_with_traffic_light_timeout = + decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; @@ -1017,6 +1026,19 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } return state_machine.getState() == StateMachine::State::GO; }; + auto stoppedAtPosition = [&](const size_t pos, const double duration) { + const double dist_stopline = fromEgoDist(pos); + const bool approached_dist_stopline = + (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stop_overshoot_margin); + const bool is_stopped = planner_data_->isVehicleStopped(duration); + if (over_stopline) { + return true; + } else if (is_stopped && approached_dist_stopline) { + return true; + } + return false; + }; // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation @@ -1111,13 +1133,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( debug_data_.adjacent_area = intersection_lanelets.adjacent_area(); // get intersection area - const auto intersection_area = planner_param_.common.use_intersection_area - ? util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr) - : std::nullopt; - if (intersection_area) { - const auto intersection_area_2d = intersection_area.value(); - debug_data_.intersection_area = toGeomPoly(intersection_area_2d); - } + const auto intersection_area = util::getIntersectionArea(assigned_lanelet, lanelet_map_ptr); auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); @@ -1143,8 +1159,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } if (initial_green_light_observed_time_) { const auto now = clock_->now(); - const bool exist_close_vehicles = - std::any_of(target_objects.all.begin(), target_objects.all.end(), [&](const auto & object) { + const bool exist_close_vehicles = std::any_of( + target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), + [&](const auto & object) { return object.dist_to_stop_line.has_value() && object.dist_to_stop_line.value() < planner_param_.collision_detection.yield_on_green_traffic_light @@ -1194,27 +1211,30 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const double occlusion_dist_thr = std::fabs( std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); - const auto blocking_attention_objects = target_objects.parked_attention_objects; - for (const auto & blocking_attention_object : blocking_attention_objects) { - debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); - } - // debug_data_.blocking_attention_objects.objects = blocking_attention_objects; - const bool is_occlusion_cleared = + auto occlusion_status = (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) - ? isOcclusionCleared( + ? getOcclusionStatus( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - blocking_attention_objects, occlusion_dist_thr) - : true; + target_objects, current_pose, occlusion_dist_thr) + : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( - is_occlusion_cleared ? StateMachine::State::GO : StateMachine::STOP, + occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, logger_.get_child("occlusion_stop"), *clock_); const bool is_occlusion_cleared_with_margin = (occlusion_stop_state_machine_.getState() == StateMachine::State::GO); - // distinguish if ego detected occlusion or RTC detects occlusion const bool ext_occlusion_requested = (is_occlusion_cleared_with_margin && !occlusion_activated_); + if (ext_occlusion_requested) { + occlusion_status = OcclusionType::RTC_OCCLUDED; + } const bool is_occlusion_state = (!is_occlusion_cleared_with_margin || ext_occlusion_requested); + if (is_occlusion_state && occlusion_status == OcclusionType::NOT_OCCLUDED) { + occlusion_status = prev_occlusion_status_; + } else { + prev_occlusion_status_ = occlusion_status; + } + // Safe if (!is_occlusion_state && !has_collision_with_margin) { return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; @@ -1225,11 +1245,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } // Occluded + // occlusion_status is assured to be not NOT_OCCLUDED const bool stopped_at_default_line = stoppedForDuration( default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, before_creep_state_machine_); if (stopped_at_default_line) { - // in this case ego will temporarily stop before entering attention area + // if specified the parameter occlusion.temporal_stop_before_attention_area OR + // has_no_traffic_light_, ego will temporarily stop before entering attention area const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( @@ -1246,20 +1268,51 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( temporal_stop_before_attention_required, closest_idx, first_attention_stop_line_idx, occlusion_stop_line_idx}; } + // following remaining block is "has_traffic_light_" + // if ego is stuck by static occlusion in the presence of traffic light, start timeout count + const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; + const bool is_stuck_by_static_occlusion = + stoppedAtPosition(occlusion_stop_line_idx, planner_param_.occlusion.before_creep_stop_time) && + is_static_occlusion; + if (has_collision_with_margin) { + // if collision is detected, timeout is reset + static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); + } else if (is_stuck_by_static_occlusion) { + static_occlusion_timeout_state_machine_.setStateWithMarginTime( + StateMachine::State::GO, logger_.get_child("static_occlusion"), *clock_); + } + 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 IntersectionModule::Safe{ + closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + } + // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED + const double max_timeout = + planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + + planner_param_.occlusion.stop_release_margin_time; + const std::optional static_occlusion_timeout = + is_stuck_by_static_occlusion + ? std::make_optional( + max_timeout - static_occlusion_timeout_state_machine_.getDuration() - + occlusion_stop_state_machine_.getDuration()) + : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); if (has_collision_with_margin) { return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, collision_stop_line_idx, first_attention_stop_line_idx, - occlusion_stop_line_idx}; + occlusion_stop_line_idx, + static_occlusion_timeout}; } else { return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, collision_stop_line_idx, first_attention_stop_line_idx, - occlusion_stop_line_idx}; + occlusion_stop_line_idx, + static_occlusion_timeout}; } } else { const auto occlusion_stop_line = @@ -1359,6 +1412,7 @@ util::TargetObjects IntersectionModule::generateTargetObjects( auto & container = is_parked_vehicle ? target_objects.parked_attention_objects : target_objects.attention_objects; if (intersection_area) { + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( @@ -1366,18 +1420,18 @@ util::TargetObjects IntersectionModule::generateTargetObjects( planner_param_.common.consider_wrong_direction_vehicle, planner_param_.common.attention_area_margin, is_parked_vehicle); if (belong_attention_lanelet_id) { - const auto id = belong_adjacent_lanelet_id.value(); + const auto id = belong_attention_lanelet_id.value(); util::TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); target_object.stop_line = attention_lanelet_stoplines.at(id); container.push_back(target_object); - } else if (bg::within(obj_poly, intersection_area_2d)) { + } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { util::TargetObject target_object; target_object.object = object; target_object.attention_lanelet = std::nullopt; target_object.stop_line = std::nullopt; - container.push_back(target_object); + target_objects.intersection_area_objects.push_back(target_object); } } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( object_direction, attention_lanelets, @@ -1395,12 +1449,12 @@ util::TargetObjects IntersectionModule::generateTargetObjects( } } for (const auto & object : target_objects.attention_objects) { - target_objects.all.push_back(object); + target_objects.all_attention_objects.push_back(object); } for (const auto & object : target_objects.parked_attention_objects) { - target_objects.all.push_back(object); + target_objects.all_attention_objects.push_back(object); } - for (auto & object : target_objects.all) { + for (auto & object : target_objects.all_attention_objects) { object.calc_dist_to_stop_line(); } return target_objects; @@ -1467,7 +1521,7 @@ bool IntersectionModule::checkCollision( // check collision between predicted_path and ego_area bool collision_detected = false; - for (const auto & target_object : target_objects->all) { + for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; // If the vehicle is expected to stop before their stopline, ignore if ( @@ -1566,14 +1620,14 @@ bool IntersectionModule::checkCollision( return collision_detected; } -bool IntersectionModule::isOcclusionCleared( +IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const std::vector & blocking_attention_objects, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1582,7 +1636,7 @@ bool IntersectionModule::isOcclusionCleared( const auto first_attention_area_idx = util::getFirstPointInsidePolygon(path_ip, lane_interval_ip, first_attention_area); if (!first_attention_area_idx) { - return false; + return OcclusionType::NOT_OCCLUDED; } const auto first_inside_attention_idx_ip_opt = @@ -1697,6 +1751,10 @@ bool IntersectionModule::isOcclusionCleared( // re-use attention_mask attention_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); // (3.1) draw all cells on attention_mask behind blocking vehicles as not occluded + const auto & blocking_attention_objects = target_objects.parked_attention_objects; + for (const auto & blocking_attention_object : blocking_attention_objects) { + debug_data_.blocking_attention_objects.objects.push_back(blocking_attention_object.object); + } std::vector> blocking_polygons; for (const auto & blocking_attention_object : blocking_attention_objects) { const Polygon2d obj_poly = tier4_autoware_utils::toPolygon2d(blocking_attention_object.object); @@ -1808,8 +1866,9 @@ bool IntersectionModule::isOcclusionCleared( }; struct NearestOcclusionPoint { - int64 division_index; - double dist; + int64 division_index{0}; + int64 point_index{0}; + double dist{0.0}; geometry_msgs::msg::Point point; geometry_msgs::msg::Point projection; } nearest_occlusion_point; @@ -1848,7 +1907,6 @@ bool IntersectionModule::isOcclusionCleared( acc_dist += dist; acc_dist_it = point_it; const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); - // TODO(Mamoru Sobue): add handling for blocking vehicles if (!valid) continue; const auto pixel = occlusion_mask.at(height - 1 - idx_y, idx_x); if (pixel == BLOCKED) { @@ -1858,7 +1916,7 @@ bool IntersectionModule::isOcclusionCleared( if (acc_dist < min_dist) { min_dist = acc_dist; nearest_occlusion_point = { - std::distance(division.begin(), point_it), acc_dist, + division_index, std::distance(division.begin(), point_it), acc_dist, tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; } @@ -1867,11 +1925,27 @@ bool IntersectionModule::isOcclusionCleared( } if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { - return true; + return OcclusionType::NOT_OCCLUDED; } + debug_data_.nearest_occlusion_projection = std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); - return false; + LineString2d ego_occlusion_line; + ego_occlusion_line.emplace_back(current_pose.position.x, current_pose.position.y); + ego_occlusion_line.emplace_back(nearest_occlusion_point.point.x, nearest_occlusion_point.point.y); + for (const auto & attention_object : target_objects.all_attention_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + for (const auto & attention_object : target_objects.intersection_area_objects) { + const auto obj_poly = tier4_autoware_utils::toPolygon2d(attention_object.object); + if (bg::intersects(obj_poly, ego_occlusion_line)) { + return OcclusionType::DYNAMICALLY_OCCLUDED; + } + } + return OcclusionType::STATICALLY_OCCLUDED; } /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index beb13ef7bef7a..c604f01c7a86b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -141,9 +141,17 @@ class IntersectionModule : public SceneModuleInterface } absence_traffic_light; double attention_lane_crop_curvature_threshold; double attention_lane_curvature_calculation_ds; + double static_occlusion_with_traffic_light_timeout; } occlusion; }; + enum OcclusionType { + NOT_OCCLUDED, + STATICALLY_OCCLUDED, + DYNAMICALLY_OCCLUDED, + RTC_OCCLUDED, // actual occlusion does not exist, only disapproved by RTC + }; + struct Indecisive { std::string error; @@ -172,6 +180,7 @@ class IntersectionModule : public SceneModuleInterface size_t first_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; + // A state peeking to occlusion limit line in the presence of traffic light struct PeekingTowardOcclusion { // NOTE: if intersection_occlusion is disapproved externally through RTC, @@ -182,7 +191,12 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stop_line_idx{0}; size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; + // 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 and shows up + // intersection_occlusion(x.y) + std::optional static_occlusion_timeout{std::nullopt}; }; + // A state detecting both collision and occlusion in the presence of traffic light struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; @@ -191,6 +205,9 @@ class IntersectionModule : public SceneModuleInterface size_t collision_stop_line_idx{0}; size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; + // 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}; }; struct OccludedAbsenceTrafficLight { @@ -262,6 +279,7 @@ class IntersectionModule : public SceneModuleInterface bool is_go_out_{false}; bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; + OcclusionType prev_occlusion_status_; // Parameter PlannerParam planner_param_; @@ -276,6 +294,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; StateMachine temporal_stop_before_attention_state_machine_; + StateMachine static_occlusion_timeout_state_machine_; // for pseudo-collision detection when ego just entered intersection on green light and upcoming // vehicles are very slow @@ -317,14 +336,14 @@ class IntersectionModule : public SceneModuleInterface const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); - bool isOcclusionCleared( + OcclusionType getOcclusionStatus( const nav_msgs::msg::OccupancyGrid & occ_grid, const std::vector & attention_areas, const lanelet::ConstLanelets & adjacent_lanelets, const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const std::vector & blocking_attention_objects, + const util::TargetObjects & target_objects, const geometry_msgs::msg::Pose & current_pose, const double occlusion_dist_thr); /* diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 403509a926022..d3b2ca3baceb4 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1279,7 +1279,7 @@ void cutPredictPathWithDuration( util::TargetObjects * target_objects, const rclcpp::Clock::SharedPtr clock, const double time_thr) { const rclcpp::Time current_time = clock->now(); - for (auto & target_object : target_objects->all) { // each objects + for (auto & target_object : target_objects->all_attention_objects) { // each objects for (auto & predicted_path : target_object.object.kinematics.predicted_paths) { // each predicted paths const auto origin_path = predicted_path; diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 3f09b54f88be4..fdcf05a97a7a9 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -42,7 +42,6 @@ struct DebugData std::optional pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; - std::optional intersection_area{std::nullopt}; std::optional ego_lane{std::nullopt}; std::optional> adjacent_area{std::nullopt}; std::optional stuck_vehicle_detect_area{std::nullopt}; @@ -57,6 +56,7 @@ struct DebugData nearest_occlusion_projection{std::nullopt}; autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; struct InterpolatedPathInfo @@ -191,7 +191,7 @@ struct TargetObjects std::vector attention_objects; std::vector parked_attention_objects; std::vector intersection_area_objects; - std::vector all; // TODO(Mamoru Sobue): avoid copy + std::vector all_attention_objects; // TODO(Mamoru Sobue): avoid copy }; enum class TrafficPrioritizedLevel { From 89ae3bbcb0ca6dd11c3178c667ae404cc1575da7 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <48479081+mehmetdogru@users.noreply.github.com> Date: Fri, 20 Oct 2023 15:36:31 +0300 Subject: [PATCH 144/206] fix(route_handler): filter out start lanelets wrt start checkpoint orientation (#5358) Signed-off-by: Mehmet Dogru --- planning/route_handler/src/route_handler.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index cf309a294d81f..d74a8712a6e3f 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -14,6 +14,7 @@ #include "route_handler/route_handler.hpp" +#include #include #include #include @@ -2127,10 +2128,24 @@ bool RouteHandler::planPathLaneletsBetweenCheckpoints( double shortest_path_length2d = std::numeric_limits::max(); for (const auto & st_llt : start_lanelets) { + // check if the angle difference between start_checkpoint and start lanelet center line + // orientation is in yaw_threshold range + double yaw_threshold = M_PI / 2.0; + bool is_proper_angle = false; + { + double lanelet_angle = lanelet::utils::getLaneletAngle(st_llt, start_checkpoint.position); + double pose_yaw = tf2::getYaw(start_checkpoint.orientation); + double angle_diff = std::abs(autoware_utils::normalize_radian(lanelet_angle - pose_yaw)); + + if (angle_diff <= std::abs(yaw_threshold)) { + is_proper_angle = true; + } + } + optional_route = routing_graph_ptr_->getRoute(st_llt, goal_lanelet, 0); - if (!optional_route) { + if (!optional_route || !is_proper_angle) { RCLCPP_ERROR_STREAM( - logger_, "Failed to find a proper path!" + logger_, "Failed to find a proper route!" << std::endl << " - start checkpoint: " << toString(start_checkpoint) << std::endl << " - goal checkpoint: " << toString(goal_checkpoint) << std::endl From 98c24b824c68605c28513de47a36aaf5a4a89817 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 21 Oct 2023 11:00:34 +0900 Subject: [PATCH 145/206] perf(motion_utils): faster removeOverlapPoints (#5360) Signed-off-by: Takayuki Murooka --- .../include/motion_utils/trajectory/trajectory.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index dcdefe61e4000..5d773c5be32d9 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -177,8 +177,7 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) for (size_t i = start_idx + 1; i < points.size(); ++i) { const auto prev_p = tier4_autoware_utils::getPoint(dst.back()); const auto curr_p = tier4_autoware_utils::getPoint(points.at(i)); - const double dist = tier4_autoware_utils::calcDistance2d(prev_p, curr_p); - if (dist < eps) { + if (std::abs(prev_p.x - curr_p.x) < eps && std::abs(prev_p.y - curr_p.y) < eps) { continue; } dst.push_back(points.at(i)); From 1502e66edba93f8e39cf9e1e190c947d32b17702 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 23 Oct 2023 14:12:54 +0900 Subject: [PATCH 146/206] feat(intersection): use own max acc/jerk param (#5370) Signed-off-by: Mamoru Sobue --- .../config/intersection.param.yaml | 3 +++ .../behavior_velocity_intersection_module/src/manager.cpp | 4 ++++ .../src/scene_intersection.cpp | 3 ++- .../src/scene_intersection.hpp | 3 +++ .../behavior_velocity_intersection_module/src/util.cpp | 8 +++----- .../behavior_velocity_intersection_module/src/util.hpp | 3 ++- 6 files changed, 17 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index be8e822b31d5c..fde55dc7a6c55 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,9 @@ use_intersection_area: false path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false + max_accel: -2.8 + max_jerk: -5.0 + delay_response_time: 0.5 stuck_vehicle: turn_direction: left: true diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0b4131b55ed90..a65e06eedcdf0 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -62,6 +62,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); + ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + ip.common.delay_response_time = + getOrDeclareParameter(node, ns + ".common.delay_response_time"); ip.stuck_vehicle.turn_direction.left = getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 1bbff9ecaf048..4def152567b32 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -987,7 +987,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, - peeking_offset, path); + planner_param_.common.max_accel, planner_param_.common.max_jerk, + planner_param_.common.delay_response_time, peeking_offset, path); if (!intersection_stop_lines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c604f01c7a86b..764f5bd7fe058 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -58,6 +58,9 @@ class IntersectionModule : public SceneModuleInterface bool use_intersection_area; bool consider_wrong_direction_vehicle; double path_interpolation_ds; + double max_accel; + double max_jerk; + double delay_response_time; } common; struct StuckVehicle { diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index d3b2ca3baceb4..27310f2129937 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -269,7 +269,8 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double peeking_offset, const double max_accel, + const double max_jerk, const double delay_response_time, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; @@ -350,11 +351,8 @@ std::optional generateIntersectionStopLines( // (4) pass judge line position on interpolated path const double velocity = planner_data->current_velocity->twist.linear.x; const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold; - const double max_stop_jerk = planner_data->max_stop_jerk_threshold; - const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); + velocity, acceleration, max_accel, max_jerk, delay_response_time); int pass_judge_ip_int = static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index ef658a25fce55..125d3bdfb570a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -70,7 +70,8 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double peeking_offset, const double max_accel, + const double max_jerk, const double delay_response_time, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon( From 6832af073897f841f8901dd8c2038ffee7318d72 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 23 Oct 2023 15:07:59 +0900 Subject: [PATCH 147/206] fix(start_planner): fix invalid lane id access (#5372) Signed-off-by: kosuke55 --- .../src/scene_module/start_planner/start_planner_module.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index ed14292747e68..6b56c88eb942a 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -624,6 +624,9 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId std::vector lane_ids; for (const auto & p : path.points) { for (const auto & id : p.lane_ids) { + if (id == lanelet::InvalId) { + continue; + } if (route_handler->isShoulderLanelet(lanelet_layer.get(id))) { continue; } From eca2d255e16aa3cd1b96a9ec7c09fa8e1b626af1 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Mon, 23 Oct 2023 16:45:19 +0900 Subject: [PATCH 148/206] fix(behavior_path_planner): fix iteration num initial value (#5369) fix(behavior_path_planner): fix iter num initial value Signed-off-by: kminoda --- planning/behavior_path_planner/src/planner_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 442049654d64e..79e6055d26a81 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -83,7 +83,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da return output; } - for (size_t itr_num = 0;; ++itr_num) { + for (size_t itr_num = 1;; ++itr_num) { /** * STEP1: get approved modules' output */ From 911a2727bcb17760ffc5978ba3570a87e2c36089 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 23 Oct 2023 17:19:30 +0900 Subject: [PATCH 149/206] feat(processing_time_checker): add a script to display processing time (#5375) * feat(processing_time_checker): add script to visualize processing time Signed-off-by: Takamasa Horibe * add maintainer Signed-off-by: Takamasa Horibe * update readme Signed-off-by: Takamasa Horibe * add empty case Signed-off-by: Takamasa Horibe * fix format Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- planning/planning_debug_tools/CMakeLists.txt | 1 + planning/planning_debug_tools/README.md | 23 ++++ .../image/processing_time_checker.png | Bin 0 -> 162295 bytes planning/planning_debug_tools/package.xml | 1 + .../scripts/processing_time_checker.py | 104 ++++++++++++++++++ 5 files changed, 129 insertions(+) create mode 100644 planning/planning_debug_tools/image/processing_time_checker.png create mode 100755 planning/planning_debug_tools/scripts/processing_time_checker.py diff --git a/planning/planning_debug_tools/CMakeLists.txt b/planning/planning_debug_tools/CMakeLists.txt index bf7e36c4c0c0a..58f73ca3836c2 100644 --- a/planning/planning_debug_tools/CMakeLists.txt +++ b/planning/planning_debug_tools/CMakeLists.txt @@ -48,6 +48,7 @@ ament_auto_package( ) install(PROGRAMS + scripts/processing_time_checker.py scripts/trajectory_visualizer.py scripts/closest_velocity_checker.py scripts/perception_replayer/perception_reproducer.py diff --git a/planning/planning_debug_tools/README.md b/planning/planning_debug_tools/README.md index fe9615614de81..aa46c0e2fc7ef 100644 --- a/planning/planning_debug_tools/README.md +++ b/planning/planning_debug_tools/README.md @@ -4,6 +4,8 @@ This package contains several planning-related debug tools. - **Trajectory analyzer**: visualizes the information (speed, curvature, yaw, etc) along the trajectory - **Closest velocity checker**: prints the velocity information indicated by each modules +- **Perception reproducer**: generates detected objects from rosbag data in planning simulator environment +- **processing time checker**: displays processing_time of modules on the terminal ## Trajectory analyzer @@ -241,3 +243,24 @@ ros2 run planning_debug_tools perception_replayer.py -b ``` Instead of publishing predicted objects, you can publish detected/tracked objects by designating `-d` or `-t`, respectively. + +## Processing time checker + +The purpose of the Processing Time Subscriber is to monitor and visualize the processing times of various ROS 2 topics in a system. By providing a real-time terminal-based visualization, users can easily confirm the processing time performance as in the picture below. + +![processing_time_checker](image/processing_time_checker.png) + +You can run the program by the following command. + +```bash +ros2 run planning_debug_tools processing_time_checker.py -f -m +``` + +This program subscribes to ROS 2 topics that have a suffix of `processing_time_ms`. + +The program allows users to customize two parameters via command-line arguments: + +- --max_display_time (or -m): This sets the maximum display time in milliseconds. The default value is 150ms. +- --display_frequency (or -f): This sets the frequency at which the terminal UI updates. The default value is 5Hz. + +By adjusting these parameters, users can tailor the display to their specific monitoring needs. diff --git a/planning/planning_debug_tools/image/processing_time_checker.png b/planning/planning_debug_tools/image/processing_time_checker.png new file mode 100644 index 0000000000000000000000000000000000000000..fb064372796ae1729d600f82e9fb0ee6cf4c1138 GIT binary patch literal 162295 zcmbTdWmH>R*EWn4D8*VRZY@QMyIWhdEmGVyNO5;a+EU!5xRl}=Jh;1-;w~Ws2<{N% zJ3d%E-x6-fGJ=1m;Jv`MXr+N?fC3xRHjQ+ahqxIqStFAXh zub<>-w{dI|i*Cxu>@>Hs2z*en4aR8JeEKc#)3YyM9?I|Ye?@s7&4avwDzGp!nV=C} z`(?-xSxlu>j?P(b43(5{yRoW65^yog$G92Fl|v3$~rXSbox`CBw5bxNb<*2 zVzP(#1#D}y<4QkHQgD&@x6c`q7Y{e+_ixWquzmU;)5TUozY@P7yoo$>mq%r51o;HS zaI(|$vsjYb8t+?MC{T>fXO36^Dy33w&xJJ_UErHUGqinm4C%v?VxzpgMZa3S%-V|= z?wDvJ(>q!sNnMb+lNAWiS~^6|r4=l&zcdxe6I5mUr-+2NugP1<6lBd=$;zkL?fDt? z5;aqGex)2f3lG(k`Ma`mgwRAUy>`Z%BrS<4Aq>txL$an0Eq{XgYEJCP;qJG%B^&7o zhn3zlPM%vy_5)abnkVq?Kx658TT7FXjD=s~TgzSj=ht>DmI#`Zo>=b0F+1G##b(~N zKrqdE#$jufx$>jiI`8T&H)@E{;%658=-OyFXMNbcqDFV#QOq1McVdF1p-fC%O&X|( zgK9qveD(Hu&7Zfqc7PEqfaiX1K*a=0zO*%n#?wzp&%U~ zlIFfk6uzzVqRt>7BAL|>IxTiML|a|&^1!@jVKj5L)@RUzlh?vt19L$#jX0`R`pp@Z ztY+TpumPPhy_WUe!bYLXD<)xZ!t>QcCbFwdNmsuO4SS*I6u+hp4}()L>W)CgQ|!@*+Z0RcwMgT)?YX3IF)^*^k>2ANZtZOwadZ@YBr zOfPl1;j!7YfWidj7Khv6Mi!1?;T-9}6#eUm9_J;2{#p8`1^R>B>ejPC2wA)wYp=n` z0NTSNJ>>)4xbu35nwP@f|Im58RFYaS|uxXEF=&1FLCv3-B=vDW)!^+0U^x+K7{@B*H=Uu>Kx_x_* zfcbrdf;pDswN4bl&U1<9-*{@aKk%Qo4*+KW6cXXmHAxsZ$`rdx!rOL_eP-J#mTsrzT^2;Jp1~X z`@StBup&b?vAI7OcV%r%jMAgr(2>0{3@Bro}Z z1M#d<^|HIdFUaPbth}ZaXa_t%V5a8r!;52GFPchlcEP;XPcpCBTB+$999mFw+*zmL zdeMjz+4uhP(xK)xgFH?e+4+J8gmzwY=@Ws_)CANNk_JiP+zR(!$G#mh*UR0C!F+=R zcurKx-8rAxmwBDMDJ;Wxl;gswd5DFM15<@vHlbqYppdR117+mu`OjSEJ%+qWlcTUE z$PZzyrk^3&P?P@(ZR)xQWomcHI{Y4h4vPTXWN6yuk$NbpIv3lwkpLHW_hE~x39F!m zsa(BjF$dDWqv#XVXO-CbY3d6V^TD*yh+dD>TI_= zS&{X_=EegI_uD>7+Fj^^9CG|8L5`2_N|xThz` zGW+h`pieq0oFzfLZ_a@RS~e}VRkZcBl#ZcI7-*KHkY44!JkA$}?7{J@pNSGRr+X zzYBU#y=Uy{ZK+!04wL8#4M9VE6q)|a4j6f z6OV1o;~{yKtEqBihjSz&O>iCit)#|-nrQQ|KSUuvb3}JZyl?putUiUBJ!tpMebRKFOfZD8P&;+(k#b?81=DKbf?Xg%*rz zpTJ4bdZ*pOD8~hl;A`peRtw<`63^IUc~qjSIhnOTS_hX-hsJH2v+uP{K(vX3KF@>q z`un*t(O?Bcai!|N3SZ?44c4Bf%4gA-1jYsyR`dSuVPWA+a|@qmP6o0EQ`lIQdjut`aXz$` zI{CeI`(F|+aAsha|q*OKoo>zu@7;vD{t z5nm+IP)|8XWfX2Oqq|c1C+N_pUGm;4{cPrNZmclH6S6lc#wH6VV*7|k(nityw77u3 z^Q3Ux`DtBIxyIk)?D{fGTjVV5Au!#Z$_Q`j?u26>T=W*4Mg1~St1cw>O}IItV{2$$ zJ5>#l*7F^BTDR*T~hIDzoqeb8Vn+^<)|U7|OlP-*8FN6>8_xe9L}h_+Jkelr)79({3E+ z(6GgH!KJd`A+K@*)GkWXHo>)wFZ5P%O!Q zfC~6%qpLS57Udg7=74$BVjlzhkA>bOZIL@mp|A{Ze+cbGa z*Q@hE$cSWtWj{8v`A6Cwsac~3+6##A%xfNkrYhP2jcgS-VsnC;j!={LI9u7v&V{b< zmdsi}G$M76OrD7L)hd95V$0SL5_7_0sVXWby1=Iix)ND-;!rl6nAjE31Jeqgqz;wt zhg&N3j<6XxMK}8xK@+@YX*P;SVc>A+oJ9*-0{fqEw>mOW_+gO(ngFrj1Gt50m+Ea= z4pDz#%{}m9NJi`QJ*vYfMdeZ|e_h=F(wm=r*I=nhn*aFoVz|Ucl)0(|>Ox8I;b-O6~JC%NfD$C3|*L6g+zM7EfD4|1!u|I~_1Cq1 zuV&bMIKOtc_2OO$D&@neGF>iA8+OJjKUObyJ2#56l@z)8s)G%glKFJQFRAvyMJi!M z+!Z-f;NVRD`1QHp5~asYcIKmSWI)HN%{F9^S_@siSj|08x;mHeTaAwA39KA${0ZNt zN%J+eTcg8|Ew_HcyX+aYPqH&IU5YSvnFZI)W%{U_<|wi*)et)A%k$AMeYB*gVptG;yJDh?*!ts;HT7QOM! z1SC}@-ft1Az+E{&Mhb2-!_Z>e_`y9v7A!$q<88)zPoHxNUMv^$+H}H_(0@-!xx12R?Y`0 z9okm-I^U-(-))Vp#b@8f;#`p=B#YU~?@+OHmSk``F;=C+L zOFa1SQ38>o@-C)hbX3Q6_$3|2p&pL$;tx$INrmo0hbHs^sP|LF1GG;SIx=Nb#tH`$ zP|3*m5=~K6MWQHayl3bm?FMH{;LP>N{5yKQHm#6^L_?)kiPA!~{@A)T0wB#fY8-sO zqxqIodC$Xe>BnESY?iAX=zN>LbQ-Et2gi328{Q)WhVQ7=_Jpn^`JyW`C3}$?K~B)a**$fM_soQ2 zM{fdenF6;uWsjgK{y~`cI;=jJ%+d&Ap}#(0F;YYZxf*w-gxn8G+nf0T9S*5^5I?GF zeNk|~e8a3>96FIRq}%IBwQ`Y{U;R^kE6+3ChF|6e2KnOu;?E9#+T#$846B*CIcU?y z#YZJ-TQM%h6R@taKP=|nvwPKJfRaRq2=*LY>Pn@%aOCW(oqqs!HTza`kt>i}r_IF9H8Sy@&0_4FYh`&Y<80lX zyfnS@k<8=}`J{OL?NE#J#Xv}QXP?g(nd*9S7T4yCnDg{E#<%CbcOcpdbFD|W?JdnL=1}BLFdOtD?h67DAk9m7k}%iU5_Pq{0+6(N0Z;XncZHJqL{A~&j>*`c2?s>*xVM>Z#5`>R*$#?sSarnqIj^Y zrGAALICHH#^}uAOca`xivgF;kk2E2Tt*&<@hnjhjCY^{Mr5>b#S4x;OH&pF&Eo51` zvBu$e{i}w0D?fbk-VON`gmsM;(kTk(Kc4?QJ}IM+nLC?gyS_k|K)SQ{YhJSD${Ul; z3+Ag-*ojia>$MlrhqY?*QfSWgHssEKQ4G9SqHa#rYC5vsr>Xmjf_o)U>P6G~F%7Kg zYe2^_`Mu-i20(nL?ltJS>TZwcZgn4|@dQvJp=LJ^xa?6&+RY?p$SQAG1DaC>;+{4} zx&D%$Ye+6dxB#h4iqd4k$2R4M>5XJ}Ta0LJ%Q%9o;B$Jo!CcJ#ZPGhauBkokVWU<~ z24d|NWASjW@Qg$cvjKzg8KN2Gj=-B0Dr03D&@-G@(k|ZkyYeSrAYPv9Q9HP13l&0o z(NRh7U?!t)G}*KVQWouh7GZQ#D*RRs^OtI3-}A0{Dq9Ebt651I7F!H1VphUIpk{ae zDzZeNL(i*YAb5FR`g=`-BN8ZI9O2I2cS3Zb)d_B^6hXZ;MDO!lz zc$d*T@fK9UFqXBQOzLzQ93#C?-CmcBycf9l%!7?4zDNsY{XtN#AYgUJPF(~_zw;G- zxv2$dkh<5|J6UwFuV+8JdB6*sG+^Du{ytQcYyJs{(oIg`s8DSPrA_%K;$Bm9vs=EN zH?k(h3^VODo2BlMJmbTxUj;h zud*8GB`E@l7q5US%-5$QjYGT!WD<{1rsd}qq%P`v6n`k$2~Et`)Pf%Iy4!&EiA$)PA<%^! z3qRz;$a_!T3OjPHYV5&YU{|boK>G5xQNkq!R|b}@2QvbeEUh;!v3RU^ChejQ$DN@^ z)q6lRb7yJROoj#WA4L;6?_9;gT5kg;ecCX86i%1ouookpyIC?}+G`A%-;PziH}6A* z=zSOs)&iD%4)^wYZ(*s4sOv77dcY+4Ak4`BZXmcOD8ua0HKM?GpLyMkxZGG_q?c5E z%K=@x4fBzq{oECKIAv2bGqn;d-adaE5hIb-y5rKbBpLz>$awV|xz|5dU}NuOu{nLq z=K4%eSc{$~=JajwRfqlA#;w04<+I0qNbvX5dl_I!ojAc=#?@|KO5T`f_&G^n&+1aJ z!gH%%j%VTQ7HPJ5EVEWOb9!cvMn#2ruFOM|9B1RMOMDYlJMwa4?5RxwgMb+*kb`)p zyNsFy6d?42TIlqh(bL9Z;8gfwcWXeR{DTN;CL_Rk( zltl2?#&g_T4Zy#QT&9b)m#{>0xIm5Ho>a@fe|DH(8{MXl#U0W^qg$F1`-WvqI#IMb zoZoNQdMaNdO9w$=M$(3~{`EtV_?Kpxi5d{&Oi3dpO*`O(A2AaPMQ*9e3xDK>4XOxC zZ|_9%mxOtIT&Nc(#wo5?uib8EJBFz^r{hi8sS_F$%i28xI zURQ<6fS7t1ON?Q%QrY;WJj;|k+@&Ag(o7IWtTU;I=B;Wk2S=Y#_ys?6CdRfno$!DH zOs#%x47F>7Z1|XS{W_X3{$BA^NjD?}2Han;Fqgai{rA_}+p|64`O?3p{d!rYj4Xj) z?$fj_Cr92?f!D)vDMWEGA8vIgiV)FFUV7zR@9urgKcrZ3c%X1+=5gjFd#ZpHKdWZ$ zV}oX%vB-N-vKt=h++%M@897Wl%SQIe;eZ2An5KVevcE=&z)TE(71+kYMJtS(ai*MK zZ+iNBTY{VI9PMC#qEVk@OZU#H#t(~ikvv5sWn;K)%S|O|Czz^dvM??QGLq#Ug(Cz$ z6tIX)k}|DjtZ31+90Q@S&BrTEj@)dyEYJWOJ@6Z=NN4BKZFMW~T%0^cBso7zx+ms_ zRwa=03O<%M=!tj2`RH+OyysadAPH>!%8qR%1Y`LSz&~{D^#Ya7N@DX}q-0$ZjMY6l zp5$#nLPX(;vbmnA^V^8}-|9fQNOc}rl-g9GREjX{ZRX@;YCe!Ne{0k`O3V2J0Rpeh z;Yk4X(IPmT(&8@TJX#HuQ4Y@V0Qadz$LCC0bS!6{W3T8j?eVjz&PyG*BQ|9Ktg4JWmyen4k2Hji=CbOp)6`#4A8$%K z9M9dM`;WA-9K+3QsrA#HIB)U{qQPmB$d3!@QF&JEG?ZMCaU+ln1B>?z`hd;a+7R{i zXgfNmSaJKnOb5~p5tE#6xvK$Eo4PMT{ZkWpks(@1d^Sd98u?YXP$%EXb=?Mn#`JNu zH67%s(8g`k=bI_QyPJ<0GkzS+SZ-?zPsBqa1ajzY0lOaiNAsMNZ|NQ}Rf@4+8A_dR z!$O_~hTfQV#|<=2eWcz+u`y z{s5G=+EK&x+&zi^&o&p^ z4Aqr9^#NBKIQ+J$s4}RlzE`A z_-N8j^RNip-NFr_rFgJxPv1_7eZSHZt;?H7nN(YQ1Cdc89`QcJ;85@kxX-zvx823R zE%xv2HS7B3PumE$##&6#YKy0}8=F%pD))jg~>nQF6bO{u=% z-}`9W`1|?4w^RTsl#+SiB)hF!W2Hr{8UJibmBo_T7Xt%DO%JWVPwIC+rgL38bu47k zG@@G<;ckmpYh5F7!3$k($7+DgQ&AiI4ne;ccz?F|LFmTedfC=(?6x%|y=1m1=lT3e z#?jBUN39pMf4}+fZOjKJTpf%e0b?Ff$UUbDy|1u@S3KTX*}i9O%k4`jApcq(D7e5T zDWX6pgMf(g7jQi`~LBBap0--I%yS zs3jP^+7N$MOxzh`Qmxd|tbf&N-|7)1)ZJv*wk>6>nV3t+{WA(b&{8KL$%l*0c09L%TLWZf@ixkFXLP5(FDju)~WskG=i-9?PY_;D|W zHZF$EqRMB(^!@mnS9NYhzw_X~GG>y*qwEO+<*~}fQ82Uo-V5-*9_x>JeSIDNRa6aP ziWgR6j60t3 zQuF_5#FrZ%yXEV6-B>5e$#ABkFNxaBIPGhM_x*>yOry+({gdYXt)b9Sr$Kf9`-+q# zo;~lhEu`_TzQpOh9v-Ip5Zk{MhMuo!JF@Dy^v&iHvoZEAwsU}mZT=7Sw=e*1ow)Y# zeuuU5qB*Ypjm`IU!4JR3{d;>WPK9zWmf~asZN(`&;eE&16~&$nEdthmn}DiWB;JF2 zXlblSG1k+(cdmnFlGI{18?Gt8oWHyJ@4mdekPu0^J!&zk4ZF5K9t!!QUTHhomqlG{ zk_HO40+)X^_OIc95~naNZk=?!h;E@*ZKY;44;D7XigYIH|J}~JQOg;NttU+@P$6cM zDJaHV_GHNwQ?i z?h30=zF&C5ht!W0X`;ceZ&Fr7hhTg=e$0ud4&I)FF(4+G$<_}i3GdM3YvQ8IQ->eZ z*mQKNWK4m1=vZTgDqUksv0Q{QH@AsDo6Wt%L4To0pNSJsV>tyxu9(}n)D1_Uovck4 zopD<4S5HHCIR<-^q0!!r6`Pb2E7Z$hq1%4pY+QONZR_@Yu9F%&DC*%rb$uFE3(a8P zAO+3XmrrZqCRXm%J;H)86Q%lNF8$v8K3_L0kdCrak9EHdUb|5ri%`Ac50;@mCFyPXQ}iGOn{^1tU8`h!S8=z zXdky}T@wQZSObaGgqrf$kHo~or?KVU3x&JXVK*MTyoOJPM?212#He;Hdy#=}xzcJe znm!y5LPJ8JU-k9@7tys7#krDS`_{WuichWdFBZi0Cpa{re>1V}jhRnQtlIqrW^SUD zb0|6iW$h!SJu5wAkWSX*s9~3}iA`q~w=<3v2nLpzL$qSrVB_>cJGz!+$22IPcJDSI z_<4~mdr?o9YBNygTF$n)o3is4;$y^dR(#MbbxP2xu!NjyL8QzD)%Y(~-w{f~ukZyG z8k&}0--2XT`F7g*qYU37Q{=L#c;0)Cq{lk44TtS?n6ySNdZjQ>^$0I~U3TKUZlVCr zv(b4Ssx&VT>(9@RE(Fm){f*pVm+<8$Zc3I1$o+$Lc6R)1?Zw9$`FM*C%@K`DKHtWc z7Zej&yjsag$I7uFz5<^s98O;JTIDHFi!6B)MEPFg1Av}!Ooq)dRmyBxUl!WjH^i>U z_J^aOB}j*ra9z1`x4$`8yde<4X1riJ8Q!5$8LFvR4k*^!++MQaHZIY7kZN?SnU8NL zI?Z5pCY?YEtD1a8cxV&XRw!MCj^Pm~?#vdl+}tHqTpZV8&yE!}VI3wKm^-CH0dIOq zP__Oo$mG9LBBLHxWexvBO0Ykrq?!wmiQj1Aq7Nd zeYsa%*gveyYNRZL;Ba+kG3aTnh!fsNkJlh?qlrigbKJ(z=Ddg2XAK#tWe#1Dh2s}q zJ5Ouvmb=foq>KnIw_VLQt4Kdg=WQ&|sAWt+qSm(YQlu$PH4X{!jUVG48ui zh54s1&!%9kZmdaowJIYi81U?OGw_5mIi+6SePs+_PlPDKs2R8?xE>e6b)%Bu7N6yyUqVg_XMbnb* z<-K&sPYtE6TdQZa(#uC1yX8BxGi?8Yy?@xh?nYJJ?q=Zf0`5^HS5xE^AE5vkonsgR z+K3sA%)J#^GQD?Q+`AJIUCh=p4!5hY1@Aj5RTtavAUD(t7N={5#4)h4&9)}HzoHU7 z@P=Ze^_0Tsws!v~fz%O&R9n1V8~L=+$ZR@=8av0AlN+SG{v8gHY}2UGTXGd3OYQFK z%=ZmHw=YtY zy=YwAid(Le!qs+*Nhy=iU}oC4E3fESd&CFAGVOc3$>A$xwM{3^SEp8r>F*M{h(QO( zbp%Obpv3cQ6`DYLf~c8`SF82g8p(667o!>{5{(!d*#v~GzM>Me^k&N&kJqhT6HUZU zUjZx@y(wwgM`mz>=CmY6{LlrDd%ClA9+TgffTq{P0-JsKBdq0I90#2@nvwW%dA38L z$DVB0b{wnicX+wZ;pWR%4A)0j^S;7N5-vdE1j^@>ohuhl-IreO)cJ^TZZ7-~8*;$) zhY~4yX@88ojDp4z>_bbo?ej}rdVp=hDB5Sh8+s418zw87USak59V->Nk9#N?N96^* ze9K;|tCxd_rk9RD;nU6I9;oXjl6lYDzQQN`g^1(Lx?V7)3#Brsp)-9j%UhB1i|~aH zVZaEdGF&7IN>w>6kmXsrXP(xaLDaNfls~u}mP zH9vlhce+$@b$xhp-PKPxk?+JF2oHVE-s0*7=MxX;a5eKr3Z1TD-_N@) zF25Ohpne>vzLD93>V}p=es(kydQ;R{cX(mzksuy2F49|hwi|S5I;Y82zW4V1F2UV< z$T0;M^mOtA+VLXe&9IikaDHcji*V_ge#^2pnoRXy!0fDW+_mxf=Xd8%^xSh-!0PFn!|gvF{P@V{`4n>g-h7G@da;&i zW;&lXC?mDMl5+Ec{AO6GB>(2fkqt%d&~s#G86P?9<9o6}LU@jKkQml!JGTn#+ci%M z$-BhMuB%u6w7f*t3Ae#>8>x?OQC^g~d~$v?*0C-|`wf1mN<(8oL(_hE7+Y*8(9`wv zs{e zFO0KpOV*#PzG+f5!-iCa_0ZG$!jHXtt<$518LgoQ`Z_DuM|7^o;!jQG@tUMl+R)alCA)q5IKL(nl{b5z3(Y&uM7!pHG6>VAKe9v4^v?GC-CIP;1Nn>DsVOFNaf{zF$6VUa3L-5@mqN;7uc!8wMi>(HAw_(C~l+&S#E-Hn% z5x@_+X^B@izWj2^wZZLexD>WV8)gClP;Ua~cmncDMQEWE1F znWA|F+}`7{XL_BV`Va-gx$w1hDK5>!O0ES7VDa+hHEM|u3W^Jov-drSEv6}IQK%W; z1Zq+h_eu=;WZLzjy$2Hfw0#vOc6M{%Jh^uhcX>?UI844PMm1zdhtQ^0p>I$DFEi3#p>jo8NZx3*3n8UyjA+U{n)HdNOW}3C_mRPSapM)8UP76f(*#SbPogElXfSD1 z71JFoQ^Qj(LdOTrY%Z~xl;G@rpYzy?^p4o_lGZGJ+wF|6EeZ6WKWwDm@EH)9Dhtqzuz>v6Qpy zpl?g=u&g~0^1EN_6;eDr*-Iifx$$<4VQc9ZBZbO}v^(s3UN1~UMdR>lSQC27d*{7g zNCUKg!0|X;?fc`7qO&uC7)}?`r-g#7Q=5fUU6|BI6=z|BoK0dN;&V5Hy~}SaCTPev zLd#OzSW^70O)W|fWg?#xO>fUSaI7LYIiXEVW=M^T)5t`dA8R5D@VXZ)P~9bzKe?tP zI{4^nNils?z;)_>_$dcB1!S|_|if-3iIic zr~*rxpp|9Ql#F()jgnuv8<<|mG)?< zJ~PljDeGv&YshG=g7CR0Sa4=EcvdY>4s4!tJmh4yk|>q*_HwA zmWQHn!*qM|$;ZF+qIMKtE3Nli8 zh`IjMxQ}793-HM^rzXy}8so5SrViAA)9QM6Co>NXosuoOe#^?uStexZi z=<3Tb^ots!wQ%1Ycl@l}Wa_EFP)bMQsW(B@Y&y%; zO=~8Nrboj}0?{@C65(J|2j#L$44;{*c|wCm#)^?luhu}^Nne67Dz>?67UK+Wuk^=1 zsBkWAPCpoch}#B*f_z&aH7qf|8;bFbJnk6xnw%HNANKK|_i1QFRynv7jH+Oiu(Gxd zRy^*HdVbbbwK=zBvSlfGp!_DuQt^sWpe!JPo#OFZvHH7>!jF$;&izwotFa>`3x&Cf z1RCLJAa?6XH~Cv1N+0xS0VF$3gXx=E$yWzs2Sq-0Mg44s^>=!jFtS0{SSTcpN+ph$ zc1J3^tG-Eqe>GZ|;@2gOPSh2i@!)7`Cm31tyGD4wAyFJ;VZyrp@*{)x&--2Tro1>t zyd7LqyE`d9pG!r4td^943J$bxvqe1_g4}YFt29RFI(Mz4Xq#9okQG^a5bT zwk;PD`032B(OXzy2gUu?t(aF=j8moWvUg|wwi>pXH0>onF)v4l#xQ7mnh=y*bqfr2 zFACo^J-D4QNMIS!wHR7>wRFSgEbcqVmN&Z98U4Jop6x&wM-W-yY$`}``CLNFwjDtB z_V~4y^L2YSn^A>-PPIHswXFgMh-IG8-zX-nA)FaX@^f#mec{l|4tNd3)+7AoIH2L! zdjOAkux5FDnv<~r+)7^{Ic69KEw(LH%dc%~GFqJxuq|yLcIJGb4g1pL^?#v7)z%4$ zzWtF#k>jvk9#%Xb(4dqI>Nh%Bs~x-@#Iqpey#ThFM@^K@UT_Dz(G2xV(V$59w~ zEp#>dXmD_7Hg?)0H)xuhf0DxG&HUm{RFUlQ(#_WFN7iw<>+KQn3*gejcCx^WrqzHE z_JPiWQK@}ll9|d#MI=D@@(Aa$7n0Q6@@^trzqfh|Ar6z>tQCH#u<<9JJ>~lo&&+)u zr5Wv*ZDMIQmr*);1hRfSKlTi=Z3+5_KlfwkPk@`o&KjtA+^R`1d#Z&h z7U9x&HrRO@nV%FMA?ZBoWaudZCMh2_{CY*DQBFsN6LbAPd0B$}H+^mHB%`zPtzjc8 zUuQ6eMvDPfW^9wzy-!w)l@z|Vlj=q2;+@Adcwo)D(+shov4311pXP=a!(3ap=*pV- zV!#`_*}74zo4Piw*Gm>O*_$hvu`33MH&Iy!@6U& z%38WwOI_>8rPU}pH!Y5BU%_}g4~o)(QfRN&A&cu@$2nr?j^Umj!c~Cc=h@lCHM1g+ z6kC(~6N7IaC^puSoR_%?Us%PJ`2f7FTS)D5v!%SD#Y3yU*k)EGqp_C_gZ_F=1^A8bfmJ~g**h|QSBhYt0j@94L5HV<0dY1c$^ z^Qj2Mz^um3DH^mml^-zE+v$x~p35Eg6NI$;LApSaDNOE}@AqG)3wcN@n`YxNW6x_N z7aOn724!P+P&Hc)Uyj;01|K8OHbR%2q%mUOYPZHShXFfW>W%uMWKf~DaXPnifTpJN zcbK?M8L6yo85$t2%f0hUcN1<2Aq^T5NrEan<^Cw;VCQdU2~l3*q(mIBYl%dc8)Dc& zE@OqyqUwg-%Mq-73|JAR3FNNhT=E23cYMYR>%63g)sLGvE9|1yvKtO0?4{RlY#B;F zh@GkTMKxLwj-kEnBg=dNS`ymf#y$1w#Q7uhU%ijX{S`Mj7t6eHA?O=5yih0!=ju904;

Gt;~0 z_CC4w@Dn9r(Iri#uy*{VMxPkFZJJa9Aj`LuW&i>H;zZFt{6%xYRK$*a{KzJni9lIGNwe=>lm7^u$82qu_ z2+Rh1RrWC7gwvj8JW97^1+T__{c}b_7bgw83{GNs!~NznlE!%6lJiJ){hR*JeMVXQ z!?>nH2MaNwlsooAcLB%C0>Ad%1@7-+J_2O`*bafG#V-ZT9k#V;tfy`*a{TK%?bMDN z;pS6;2a-j9<^F^~h4j$Ls&9oU#;Yv$@F^6s-sax9v_-wek+K9=o5C0h_!5a5ot9Ev zT;B5XC^{G{ae|JW|H!iW0*#chsiX9K6uW==MS?}%eZly!{neS6_X|KChU8NYTU^)y zGl9zo8X6y0g($*wJ?Hx_LoPSLd#FIrFK#YQzU3%70}wT_*GAL*wsjEPB)nyst85zQ zI%D+h=zE1#U-f;-SQW^GSwEEEa3f0eL_aZI?K6=>YbK@0zVX_rGB+SjR_?F5aOjA` z{bqCoaJryE+lC8Yn@xy>(vd3?1Hhd$G^ZJ+x0BKgvN1zMrm35-wkjCq3!V}@d401d zUXWV(dAAYY*wkixyQ76~sqxb^soCkpPqKIY_hp>>$um&BB#;R$J{iUtK6kt)g$SEq zO~KdmIaRwL&SQ%kpE}@rGcLARM_r-lWGUHwJwA1Qe`AiuYN%;n!{Q%@Q$qV4!so%0 z6!#VWp~%v9N~-s5x>SI20nb*9FE@sz!=4C>yoJEbq*j)%MlPNuj;4iE4KLts zElKaVfoN|VXB*bFmYShNB=pv}QXSL5s8HH0Ek7d()Ggba72qD|#bL0$rnWH2@lS*k zZR7R~+m#SWlvUkWeXip`XRH>*aXFv;@e{2_@PG{;vN0jrCjl^DD_?fq`$9iG)vPp0 z6fk~l9{IthlZLp~;vs#7&2Xl#@ET4Y+g!2JZ$A0{IIbzL8;km=Z{{qS|NSOc^0wWT zl*93cbX{H?eJ9cSyjc`})wReCiQoq^gj!(63KhmoBzs|FZ8^ULEZF{AJRh|Ov(*4M zHr9XaAuRZ*hLza#w;sDc5BvUioEQ-0A2crx3B-Bw#^lp0*%+Jh;7LMUoXR?xvMqsE zZp8o2-aizY(k<5sC?)4Fu|L1((Tj&q8BE*JwEZASf8`~A^}fc&UF!7+5~kQv>-s~I zjmK;-!-hjipD>rE=~h$wV!*4T#+Zkt$5$3!<8DtsST^CvH`#w*UjiEM)Qh|pWhQt} z8gV?PmIeoZ7zTsl0IP0aB9W`(^-Al@gCSX4ZLeLepE^}T=)XAd120@d3}JXKEFckQ=@jLCc1_ z<}*jRj#a?dza2M_JCPdxUy_Bl+gW4^!W5@F-Q;#fR7=jG&-DKwg%JOt&f@uK^<+?@yScE{P?eq zN&c&hI~k2Q-Xp^gu_1Q&Qnw?Wo~J>p)#3|8(SIWiHA3~JGcAAb1-R>bK9MG5T_X{J zli3&)HrH*f|Cev)NMH7E4p_6d7irp_LGmNq!gP4w>EM0RyLd#UdzsRoC7Y;LSml9g z#3r=jlb!&n^&n@x1+h*`jNwm@S!uR`v897GdJGlde*N1&>P1jPDy=2RGRUYs| zm8VEj0X?sR=#{+3qfMI5xc6WJTcgJ0X4p4WkY-P zrQkabP1*n++Vj=WPu!m$pgpHwmyl;1ZX9B#dV4{gaB;1Mz`pcJdIcc`cp`40Jz4KRS|N77Y05^2)~9odcdEIy!1A zoRiUqs`f%;gYuT!mto7SwT|8^N}qx~JKmJe8&aby#OZ-wT&~3VjtXl~B2~#C5A0WYxNr!TJ;7O{;Cw%nzi4 z`hr%9SKRihOe36kSVjryGoiBCqBQVArZ*u*4Z*@e&utg{7H;+c>&jQ3sQc|tK#a7$ zhsJQVkaZ?!yP7#3T8OXUyw1X~+3-~QNXnu@K_bAxk|S8*N{sV#~2DVQ{G z&2ogHyF`8XaP%JN>UHvVk76d$V)*)?5*F*s7t4;*Y6egj*(*8yj&ZtiOF;YGQeMiJ zU9(cT_i?=~_Hs!G{L7xuR8DVw6^Js0A1|jR@!Js5*pB`Md9g>%jFynfj4TdCUp4Wa z9BD7>?jymw)O?HG9|y4=FR5_a7J_Z|>Z8V17p)d2u*5n2C%!_Z*{;7%`fwVpc3M%R z>E5`Xr1HefBZJ1+%4uZkXU*tsEoVtb1q#zg0AIJ z96cZ=H_p<^GOJQ{@kCWNe&d2@%>Aset#aa?RAt)XBvdl4HF;mlQ&A8G@oP$ca4IP} zvXa?vmFVl67agBCM=R`&Y9uftg)<@f3(MHvg5gJ)gq$qOTwrten7$*B{_WE zXi2mMOSZG#6Z=vaMoU-yjgxeQ3NX4hs#AYE(Y^b0zT+2^#_(p@i~@-;W0J1AX${kE zX04%bWeLUW-2SN|NqkC4T+^u9`phq^%f!oVvw&7PwDS>Q$eI7#_mamyW!|Vm+;&Md z5cPZ9X{V?datXC0^wd^8`J20jR6Y93?yN>TU31xdNl@=48=kWt~_~uK{gQHHHSQ8ZNc+qS$%SLMrPi;NAJh zp01{WW=EO%iH*~!Jqz{uGw$fooa7f1XzKJ zVD)REY;U%jQ4SRLPxj*$JXYl)%?`~yzo?UYz(Pes(u1p=o7RiN3os|8vaLDi!3%^j zqIbyXf^}9znwFW)bCzg9;s;(MNq3_Yo_gjLo^4uG*y_Zga~`W!U4+ur28@mU|6}hh zgWKxbbj`36W6W$PF~+2r*@|OkX12`CY&nT3M#ap`%uF%L%#1O!95Y*^ykCFQZ+iMW zea=jushXE)e1Nnte<_|rzmET%QUom@E5()?4N0*LT*Ipo=$Hy%BDA%G&ww7bf*FYiEh_zk(w zJSy?X)$T+u#3vSaO!c;+bYHZo`P}Qqf^tstHEEBu{sz6=_w!%-R?^k)t+Pz;`b{xN zyBBc8j|c(WWGpcm4h&wqT-PG0BxnjD0>)EgsAjWKV*rdCM=d?oqaoEIp+P@^J2vdw zw9ULZa=Q=A7)rix5^Jg*IF1Y{*qg19S#?`6wYqqw?@#H6lL0@*^SN8|NTaZRQJ<~i zyPG(@<=q14PaU~GJFEA#k4iIEgrSBe9Fbww~mL;AM6SyJMu#1)KnI;C>_Y??XLef7DQL+W-F>D-P@wg2Jb+i9l7p@t_u z^N@zXUut9&>lvJL(b@Ff0S305mbNVehS+#@pNGY4nYp4dXzKsyH6s9i@6x}3s?R<>7n|I z+}94S-XhgebFG^4Nb`X^T}&al;48=GlW+L@sSn>e+jh@(=E-~*Q&FeT=jlH`TJbs= z5`4xX{+1Q$;eKCaY1ZG$+D~8 zr=RLZPX@iCGO%weAFgGL3c*A+GHW}z*^##Epq3prGrGej4$%++PkY0GIf9EL0qM7P zY}0Y!14rBQsSkI_@fo#YjB(Xb4f_2wRbMY38s;&=ZtS4>`oj#K5zqCgXUQigPq*WC_(;ZtykWY_o>sp#TDT+OYIg3mXQhzduQ4M=vcWAcH3-B$VaCs2! zp>*=MvWDiD0$I?DO`j%??_@ioZT5Q~+H8PkBW2iN?fXsP#`81NQ%HiveSo0zj&9p( z=F-w%;YEXtFc7y#{jo++Uf-+U+?Vv+5_uAmb^=4$9UdHogLD?Ou8YJ;I1AOu&GHDr zHhw2HdZ{>S9<9KJUcNlbtXO%TsdSp5^!kCi zSDx>&l8~*HDatO}A^&ok`8&>s*9-XG!-X;uRP;TCR!`HfS30oi$<}$u(O5Dyt{3Un zB)6L*4poZZq+-*gTdLC!W)lE){}BDvL=_HTtZ;u8E>c$p-HRU#E)_g3ER|=eI3J%Y zjbm}$>pW*151K!_-7K@CFZzWOc$xEI?LuBtgCjy?6)>Cdm524$Seu<=83)_NOt0Jk zbU=$@s!6wDzi)5XRHAR{<*Q#p6j|d7`Ls>sN@kw(le{CaZ^Ai9?3*(Nj{|{vgQ=Y4 zCi5rzL36E9EyT5zo=4^|Iwd}ne`PeF8KYczn?L9Znu)qq~&ic}NrK&q&EsdzD~zQfMF zskD!yCS@HW^gceYCdZ$!l5W8^n<^?YZ;Y-NBTTvm+?mc(JPj7|KVy`YAW_`6SJwg6 zRsBPbbgrR9Ns{ycfO1b+zwS)TP#K&5GY;bTBhig?h3g!T6n)`PgUK3??z~OyIhVR! zBddk>2q;`~1T1rXo#c4sd?YE}ZhP0P=e=p9lIrB0@368VozsJZFue5wTtQilM?)$O zA12D<%dT|QH^B{+g&y}xSx(S23d_$j(13uxY!$f`#o>xYOO%vBUt)FV@=6-3Hvm1a zJwaU;@bejFkuE@Tcjy!~OH_$A|Kh#S92O^w>6+-r&L(q$896UMPt(@Vu3MOS7PM+eoZGcR4kPm8c`o%*)oWdnq`Hn~5g@aL9`PA9+4qIhRR! zthDkJTZxeaJ&_r8%f^04+L!&E#(+}Z+x(RdmAYl|^=N)!Vfta=@;OM^ok4v8;Zqa- zk=fY5YcQ;hizLk#Z#GP!OzzE(O~Br4@JH4L9!2hjoyH_=w*CXCRYz#dyPQ1T(*4oI za{uQTVim2fp!>wy5>M;0gm}QdP^gj88YQER^U2cbOq8 zEb_ZtPOLqP^act4&+7(&AESVD6T9yhtv$ z#B)hh&nfm62`%&*`N$u)t|G38y39uzB0u&b*m&A{)=JidQdMk4ak;`Si;+)4km=eV zQ9zT2H4p4*$w^T}-8|#C5Utw$9*b%QMtuBI?Nmv}9;CV5(Eyx1AKR{Owa06*wH#5q zMUT;VRrA%_sv#NCjlW*E?>dm`*bYfRMLz27@ez&Vj{b6Je9~SgfRj92Qb4cy_iXjG z4-4Ls>YZKgq+~IXex*;wK2c(>p4Y{$`YP@aV#d=Lh+Q_5s(ViyRqMX<<*#g>(Vh42N=+F5XR^($st`TlABpdXA$# zI(A1Sua`Bs?>mK)B1tK6B(}pIHX5Gnc;DV8Nvg@p-2mdJaV<7=PFj^t@d3D^tqk)H zM2(J+(g;imr#SMmo|XB4dPf?7!_D!ZXS!Ly_EH8I-U!wq-NBXKt6D4-ZuYx>!t%%U=4X7 ztV><+vH#WDLVCc=`pK_FW(M?-hJOfY^L>=wcL|fmaN8ohXYuG9erMcYSN}>&GurRKLC5*5N9qMUG#x)S##GZsZ+dqw|9eugO z@omB8gZ;(=+Fhz@h!Rx@7Ps@0Mkkv<`~vNZOKEfFJA29X>1CaQHZUhQkJ*FHu1AgN zcpbC$4d-K<)VSO^_(OBy!VXU(nzZc0Y;XL9!!HY=1s6M$1@jTD7WKb}urAzoLl)e! zUQG)_`p+^?vGIXyGfr%5x3xUc#haNrH^;R(mUU6!F*%)b>Enlglt*?c;k8$!B(`b7WsSDs6 ztU9VTTyHcR&emXNH!~~iw9YHffo%lF2)S=9Z@bO=uqXNOOT5W`cAi`rm8ObeI{#G3J>QPXb4{k2=mcNXFU5HOOveTo8qHNKNrHD&ezPyeNvnE3 zsZ*@Mo}71d%L_55pL2Ng+|u`gZ9IR`w;p*R^P3W|czbpU!vW4lJNv-gG|c6x)@WK& zFwGEZ>6sg~zZ5D<0sX+n7Bn>BQ?soWc1iqVGNb)I0C`u75}hB2J_Q959@#6d&b9bj+l^6UE|vxODi79Ao&0$lsoS=xHndc(8S(;B00$p;NV zlkR7qFP@p>!&8UKy*O!+oLx@_RQ}R1E{NT|1opFWiL{%i`OAlxd3<(><7B0*`ruIFP*N_f8U!KTOCtAaQY(35r zQGw+=?bLPids~tYvE^e03aENUQCatq%sU54&Nf271TR%2icHeHxOYB)T?IN6b4 zCC++lbs${2{V2qT&e410qU$WCLFD9yIaF1B#PtgvsxaF=CABiH*$P4T~AfkXi)jP&V_5r zXGafFDl)&UA4WHVHdU8$=hW<`YC>#FCm%W82A5s{qzc3W7d$ZCOpq|4+I+#~)7ym= z5Ie1MPJCH%(Ds6(dILq%VBo4q7o%Qp<7nUx{J`mjwm8+p?el{<`(SM?tfb|5nR_Vo z!&SpJ;kF(m^Nz$pag8h4?b>oZR6)372IZ>tJ_|MV29*;(+hHy6`N6VFq;`y6T~5Vq zitDla+T#ELo*1p>6A5mY;RlMAvvmmap8RRg#-`Y|LKMN`BE$pdyCcV!EWN?4Bs9Ws z#~BG7&ZN4DBmE7(hec*@+Q+R#&&^G_D_od8>(btdjch}%ZGW}T*)MWiP+me%yB1=_ zG&?Y-$?pg^cfkR5{vjUIr(73(NnXk4k6gpjI#!52nH+$pn}0nAig0_*Dn1VC?1vf@ zoGAO;6W|nW*jvGp?syM%VW?@n(YM65uziEkN*W%U)Dd)qUv4S&Rnv+Ss-(ViSH{pGGa? zQum=P+Gp}@$MW$q)H11=KxFHTO5&_xsrPNlBTK!Wj%vydqv`Q#4fv{!dQ7THGr(Z& zum-BI~voqK)()9oQ)6Tp(qUrP{&?fVpNzU`OS zad+C=cXDup+L3$d_+M(N$uASyjy97BL+0FT4}{h6w{xz4PGqyUyuer+)(0|7TPcFj ztF|`F)SCcu-o#${7=$8Aa%Ad%bbXZCvm+^Cwl=n$aJg0&yk4#Y`*On*7I~AHLJ5x zqxoc|et=v&KJmlO3f$0Yo+LG~W?**4a#~RH*N0Rcp4353GE*2eR46cbPhL5tYn6%; zy^L=>w?_CTR!W>?I6RAJU!3Ezv zPc;>GFR%hCP~-a#5{OU&;33{I=vJ&l(7~VGG+M9kr-4^a>)eD;Nmlmk1=cDzs~_C4 za_(`AL5jS8rvh&a19Q$$RiT>2RM=Y1q|o3J9?MYu32r0Du<3c*H#YvDAG@Jnd%9S!U zP2a6*^TUuW{C-AA81=6>_%Fv$X{!=ai|d?E4&~GYGrAoUIQADl)F+nANu07&;Fip9GeQMFV-VpPb8aH+|();l# zZ9u}$J(=x=(!BP*w4iS>ff;ek&qs?r^+ZwYlSCizthje6Md1BwrfqLovg)yN(2KfA z`gPJ;hJV#=IdIUvGv8$}85+qSGjxxpXGT|9@YX$uxz>kES}Dmoqjr5VkL092f^%kA+4;TxwWSxaARh2?`FusIBWghT8*j>11l|}-PPAlq@dtA zg=pV}7?(16ya+UDYd_eq(@Y(V4l3!k;!XyFKTl1lRXpg&PKNwZwpirU*bo%QNh>n4 zYpBeetvPESuI%4*OO&JY@Nnqlxf|$*;c33yZhmPs@%52yNpjjgPAX7H(LyjRR#KVM z+*M(Tvd51mKg3=ge}F+)MdoODON+%k4$(?GN%R|nJIM`Id0H_ zkv~HP(Zh`_870*+m(N}>(|r@A^+#3p_XkCIN`t9YNSZ!%LX)LUMaZ$y(DIl>U1u3c z{eNPpKYj}FZ=xD~dRo1vQOL8S*%#N1tsP=+yds^~ccA)KnAF5VVkGd$bQbL)`f_au zh&B}G;wr7*v2zh$RG(HPx;)?w;!(B+u8l~nmD@ykl=8ry3y$fZ0yvcQ^Y9iM1x3)bKS;K+JJ zl2J4;^GrDxeZO+PaJ!Nc9iYO2 z&R+vwG_e^%yF}Y;tc7jIDOj(*w+^2ET)s*{Xy-k#_9eri`h_1lscajWOkpiKr<-J3 zMu(E4*joACo+3$OciyWEZ_m-%j3~=iqGD6Ip_ILJr$e=b*Yl2=#l% z)B1`ZD&)G&L3uG6-{>k48-Wjpbe5?KuO8!f%QTM!bI4h?jMO!#7c2Lprb}B;1$k>% zz3yX0z@N`hOpe1QkRf!~Y8RYkVw@x@1Ght?#R zui~H7uQuQl7J`|k#5;NGsbub-(pnMw`LE+l_1e>`1v;x)Pn~X3qDwXLq4QUPEg})u z&TXyf>Mo18MAm;?qAE#ar_K6N_3O>BzULJ=jF{WFx&h?Qenbq_BWr?BWK~q51 z5n>cNSSNqk;3u~itV3e(FflRBS^jHe% zNc|R0ciPWSGNiSY&g!O9iu}A3_HGeXTxY#E1rnk){zx_EDVu_bC=MAU4eBf`0wEq+ zcO#gxFFhSVvf%Kv@UQf69AY2EV-LOtUR54+sj?9*9dpU?V@lqQ ziwtt38mzau7oFT)7n4-5|G{um+KKpro*El z2yCReSExmXqU`XLXX*ZtE3oR3_&;X>T6l5yvurEgR}fLB;{9L1s`GOU_dnES z0ohz*P$`)-Qay;$CJm{>1FRL|`>5+QHx{AfmM@=K#dUva5Tm7ifFzRZka6KEja#$ErLD7!#4!j<)?7fxhw2ZGRo%=8dWB%_#)xz*2g5 ze!=|M#{0g_jnqDu(!jRYFJnHq@SU^ikKGv^*wM2eSVG$ZwJe`BAW{IuYku;6)}0=@68 zB5UQ%JWi7knr;JsW|M|Q&sMJl!J$JyIXQ>z`_4zBeK)J5soqDARJ(t7mn54CGZr65 zuZ=EoFmeKLdH_?+b_sg99DXA{AzD73aoa#XBinV4mW0YJi6`3vL!R`*bB=TQUTSj? zNf@nm2+=3Q8etXKd31P=(~#|tdvuFSRa|SOr1b(NWN){KPYE)lfu~(kt4>zCA?Jh4 znVBc%oaw|e$l+CoX;m7>MWFGt$1{6{P(-?l=NyjrvCf<|fo-;HxlR|f8!o-BMxtVp zx%tb$sRm{OJK>_4ak6eOzH=@=2WiM?F=a!*xyCr`IG(8iyWga=EE%E%1F592r&$_y1R+ z>Fs|XG)0d6e*#VS+5W!>O*6Ql8^QXG=O`I(3~|LA1V>IsIoP+Oel3|6^BU*umVgLO zJ5a7R_(n{5E60}1P!<}5igoIf9G^c_lRILpmhp-92-#M?mr(mXogiPal3%{0BEIs@ zUOjNFyn~>s=cKcG*XocflEzebAcAulq*Ls@7sJSV9yz0`raU4Vqah-*8NN5Wp_>%z zdSA=}ZeT_{-~e3F&8B|vq=VR|=HzRvB6O9enW8Iox9%)U!9@N7ig@+mQYJw5VBlt(Gos+eyq-NMD=AX4H4Ji| zIgwsaeKb$NcyrJl#AphdZM*&8|6@#DkB6x(&C>Jc)X^|to}*TKTz2d0`?Nvi<%t~& zAlqIiY1UWnssj-`uLOM`d8LswEi0i{PDSWLSDYulgS3Y4Aa;C94<9KwDkrJWd6=CX z^t>qWkLN?fvZMJT1e7a-@FwFT))TSrS6RIp+AHY#$DT2y08Pffr#>sufVv|Bf)xtw z8+)k9I~U#rqxAohf_i*YmR>8TvsZUK_mwQB8@w4$Th1_PFX8l&hdJ6M+cmmOBeOS1fH29awI^ z@3nRw)_lVrLX5QE9ytWbF#f3XWPS3a#q7ZK%g&_N4vt^jD;6BtA1U`{4|Tt~f}!=p zV!okfsseSvTw?H|y!&=?8g?5Da=|?Q6$gf#mu0z(FD>++EPWN}Z0&Gy-cS?dqmlhM zQ?42>VXvw8hlYh!)PvQGryk3=7WY_W85tIZqc@u9%}vd9YSO@m77H+D>cnX*CXV9T zI!cN=lt`B2I=(FknH$duoA|fsn?pmLz*qZD$J05Mk0XlqDko+Wp9N=qn*}RSwjpLfmOueWjet|!akbXw#*)Tj zP8R27v{|N(WMa#ZGRRAB}4m5S{1xwVdW_O#DW%WJDY2&o$1 zhE*FWp0H^?zs`s#n6@QQ{x`(X{XR&@f*-xe^1wy)pL?PLNXdM2)4%^pWp2N*@;WB> z`wtV}6CGnQp^#NV70lo8pE&OeLLoO zQDN0DJ^j2cXMcxz@wy$=?M7z02j|n+tLfI7_@4>u?k1bGXYWRl9eT@5Gxe2t+>PZr zdhaSXBa+aT(e`-Zovwex`pjU4M`qn=E(^f$X=(kcIrtO_xd*<_1KP)9BlYcpT8F6; z&T%ed49u}$Iag~QF4a&KLyMlz8Y8T=n+C0Ew*YkLcKUM{(BwY$OS|h3p4xY#(xu_b z!yJvUE#G67Tjrz>pd9ht_c{AK)znr7?VSq6M@xakNXvu2W;z>AcU%XP8X`dnJUbjt zAL|-uD_$frCLETvjoj3;$k7$gW#c`j_CGCj(u&Ud>cP~ui5g$~#$H{N%AtWRYfp?y zRfTJB*tn)C>uYWALY2Sw_U)*p9l=W42glRzvB?V`ARuG4o|7{U#s`#_`TvbD68BQc z07l@=s7Z)bRN9~gq^)jJxa6f>)Ov3N0;tJ^=M{?On?3Dj+2^q0pkZA^g)rkoJHl#T zXI3IR^a1?RKi`;b=){tHXc8q%(v(+lCNc(A8+|;bD6hcC&WXz58k;Y7 zu78`0^BciuCB#{d2!n_AAZuTF^PiGk5w7%v9T%y644)a|t1;vA4}^f855|$q{K3PR(4TO3v!}ZIz7-{psRD1$Q@`zC_p4O6ghM;v3yvU5`2wo$&pf7wu1;s zlTTWR$PMx3?F7Y6EkAmPDVAZVDml?bP_~d~%&>jpbr?QJg3~#h4Q*_MpC>O?q)u4} zusT1qEV6s?9x=#$0=gz&DPw!TSo}EJC?l))R09WJ3V{$a4qL<#t0_?7{$gPkaxev$zAFFa>lbNa+FR0z7rVkqthmv&dt%#v~z|~tAp0WtEWG;8GQUFryh_X znFfBkQD(aJ%&jJP;L5SLJSv^k zYfB;_WrLSGwP;f8E)Y~Qz4y4@-dqMt;(v5)wk|jz&%}v{OO)1a!CO4B;L0Fuy55MZ z9&qRwy*v4LcxbKp_ud%-9)9Y}6d>Yox}{WQN}2Qs8b9iDItd*duL1BUX1}huhB(kb z5}MEr2X*Y&D!%1D?RgD$ruH<2y=yrWlHd=Vpc((*k73yL-3fjBYRqrlY6*<2rSR^! zLe*ngAb7#hXA837#nx8z@}#gQ2;V~>h}m=uMmCs2F+Q87n?{|BZ8hhC730r+#>zX} zOrI=9meAu}d-RRRI#;6Zi=XD3n!#GcTtkQ~HrdZalt3;#$8eG&e6%LNu~Eg|IaDZp zBnqSwEJ)d+>lSaiV&2{3JeDj`WoR)vsvEcR;uLrzV&gjc%9ZA=3!Asenn$~}=^mAvRmGGpHWgudh*nBHw#1iLZVtH*FL zEE}B9Ky2iT_X?WSt1P(WRxVx1CR8goKO#@RI5f{zXh5>Ztrfw{1FMfzG?8ySn9G;n zgIrSVFG`(8ER;$#2xcy%X=a)TQ$)iJ%*?Qo^57ODWkg4usQE&^w`jQIb>bu*V3I;; z{Gl{oP{U(wpglud5=MlvjW9}?8;7;5d@$O2|H03<4WP+cf+R zz?tR}7~kEKf4cMd@YOi2xoy_(=+r0)tDk_Cu0g{tBGuV{>W-)+sLU^w-;yHIogC|6 z6g|iF0^}TlMUey*0#Z4(tA##e=KKQ3eir9>j(c!*tcm2JeZ>Ni)Bn2 zZ3d}%1gwZ&_5>BXUprnj)g)FlUFRsY&B!=ezaHt~FJP(A{<~Na1n{$Wjq2^bEc@+V zaboz^)qJaAk@vV|pV9Q158(E!k&M)4U*u;3sW6G-F>QxtT2YTchjsh1HkWs3itUKO zX7~Y16a71z>rd1E0>W_9Jl3C|=#Eza`Xk{=8}pH~XFXPXK!Jev5&jV_xChSW_V*|( z=xW!19l@z0U;`QSFKeSV3hx>j!%WcM29np40?6f|es z;k!bweofJe^9}^ngv>SF<8XTj_ze2^Vho3`4=YeK_!zxeuH_rY3NL^7IYKLU$=b}Q zJz13st&m&ZST9DZx4gr7n10Oyx~$sr2VA``#KK^uLZ>GW%;{IUKL^PdM?xzaY)_7= zJsViP-YLOsKR0{`JiDSe;MPb8TrD-6YV}WTPH`p{bv*y3J?%p%7(Nko@ttGpl-qOX zrRfU{{c+FE0@H!Fo9dKBsanGU_uc18r}v^U`=dL54S!(;rjq@h%CMn?d-|X~hI4*L zT0s{ZkXh}zM7;QkN|2Ewl+N*$Tpwvx4|TrVpbB9dPJdRk9?$zCpn=V3=N;8u8Q*z2 zz6Y3N_gL&3SB&jS7or(65BUVEpMo*1K~o+@{3T%A7=elOThbVWes9YR+1XPOua63h z!)d-_!KWqd>Ln=ptcBQAqcbVPVG`LZ`e8fytO4-HVV+jW59W-4N8o7Qqocu9Zn56B zjcuN7pBs!SZGg{JvyN{Rp#?K(C)vbufvL3+bVjzI0QHsrymPAj#{z!aZ@|wvVb2j(+<7G{5;LLkIpMdPiuSAS7`8MOQ%tndO zS90?B>*WXncgHPJ9Nd`6)MN2`1%DKfe!rgK)70U~X)>nnY2F^XA1{zp?`eg@4L9)7wH_DN{gHapSf2h-SjoOl@hxW z<)2m8`qx6P^~=bjpzxe|^eF*3c)`@KJO0W9oxDcbZKN{3vSXX6+RO)5}A8 zBZhp7dA+sB;!M!lc(0tPsKkRwYVDVz@%pIJKGb?^deLr0zSMFb3?`hyOC8TOLwiXq z5czn9(-{>tCY$|@(~jNExtU_keSN#q4H|B0A}>~;idx*-D5HSP%fkiVTC^1HUsL-0 zo)$1=q#Vn({!l&9O*JZ=iWw~9sZVev>`+?(_Lmlz;Yst{U7NGi)d&Q>S_6Uid>&Qg z&o6p_&XK{5E@kNWY^)T`lpyp~`Hl=vb5GLHCU!KY1VvCb=JRw-<|+8iClntDD*fJn zk6p4gJO~6Zm#TgKj{w-X0?=AGM^hb}I9q6*&I?xLtEh21u?!lI(guLuT}=!48HrTV zvFJ5?^ysm{lM8N#;`~4xY3Uu6OpZQgYxdO|p9FsHs1;P_l2r&JsdI2vcb!t@*OOf< zuO3(*W!=xC{(X-oa-HM0&xq;xSX~i}yVRyTgI8J&&0w6E^YwY1Z&%*TvWB|}OgWJL zJwP(bGN^HQ=VJWHjond^&s#PRssUZT!RvSAPZBxboHmyj5ntY<(X+c2cAWO)1}{vW z_A@*b53-TpbCQvo8(bF8s5i3Sk3(7!Aawcs(K_@pQ-&feG`_{^%IsC!UF_lVK-l7< zTuV{Fht1Bwox~gVGT8@mQ>%+gj$PiwvCBp2xZ$B+RB@9lmsOi>QnPj+2Cm{5=OX9M zdy?XH6S|*Pk*yC#U(UJAs~#V{`eWCR(SM&>Zs90x#K`|eWrE%5j;$riZveIfp?UC*op zKlVKYt;m)CI_rxqj^FwVBxRzrJ_I51uZ*E){CZ0V+l#iw z5C+2jKcCB|Qpfm-F!hWYw;0s}y@j@^QN&FmrEiQ?9DaiKcWTG|?O#GW-R08c@vk0^ z@IZ`F*6g>fc=U65pl{l>ezwzHj)JZvmgDP-b8Toti7yma1|+wF%v_l6LZ^5Y59oi; zT4g;)Gnx;EXN9cCxcDc*hm@*Z6x<{XaKfMK+jOyCEk%); z42Lk8satZPTiwoK9YGN&-9%mW)rexiBYi5D;T8AoqN*L6Gayx9Nk>bbVk+H|FM%E0 zf4)Wcx9RZ;7X2v+?|t;#q&=5hGmb1YwpD$Ego2JP&E;w1SoTk@g$M2=+_qd|ywl$} zj}sqv{K*c7HFeaBq}khPONONg^Sj`ze(7owfzZ~s_j`dln-t|F7mRr)3 zUH)|{s8D_7dAi;IecNN_zY{#rBpU-|SCNwSf50Rc|rcXW=5$KNdL&c`5+=X)D6HdwHwVl;Jj#bk`gL z(;>3Hx#O*tplZYc<}|g*VFqwmC(aWwkoRu=9k}#xcVr}p&|+cML4r2{CD)6E-VKmh zbU&Jp;Tnp(HCTzS@yqz@y`T)gk4*&{)#%)Cf64z2W25nm<`bMf4NC{?&Lh;V$?cKT zxnNhsN=-#l+CUElyojEG-U^v=l5Kq~fokr&*Omz(@PN-o311W5LMj zjYnEoxT~{^1571Eg@6AMCg!#D-b~1Me|t)tF9p`N9{!sSc70$6=Z}|z9E`ya2}iAt z#gR2fl(8;HtR7Z+4|SGLcfZ)25$Bt9M&4}N&Ufl07Zi&gCt%A!Vqnr}6pKVV_(-r?^<_&Owwp2(|$PDx$+(H8>`#fsb z&V7>Qo1+h1Ggx#pv)zS@@ciVsPg;n{dclyx0t8~VRXFl_{Il<%W8kO&Qlzr)WG{ny+EfA)ej z8^G4*KXSkNac@IsM2hl10(%EhD$b+TA&%PY<-6fQ#Wy)oS}CQ=ngI<;gnHyjGqjq04gmz^p*=$x~i%vo$5S(W?$68A> zL2(W2)I`@d0Ql_dDj8zqs{_3S_y1Qqv zfK@IUORc~;?=id8i@Iu*IGRLHUe508(e`)1j@-u~whstP{%6Nrkv%*>A#8f5Q!g-bo! z#r=OHdq;neJy;>d)&MUa!Yh-{`!}+O2lJ?|t+OjfCFaZ5F5(~MU(ewvJQIBOtb_U` zUC0yG%e!47$2$(10)iSl}deZhpusfe6l3!42P!1yqb@v-^_rUwqpI9 z&nO>402)=t$`7bKWDk_vJ2Axa1(bowQap)ifOED3GV)^TBW1o<9^{|O%s;zaC9h*V zsO*g<4)-1Ib2WIA7HOdwZ>@O}usRW!m56@?x`x&pVRK$IrKZ;q4|p9I>K-V_s^4PH z4@YgtRKz4opyUVC(zd0ZhIc!3%tMMA1N4LYeJE+Sa-9zFLeI-UU229jcHR2E-ROzbikf@ZKX$3_V;30xsgO%Pz>93kyoEC24>EYA&AY2>3i zK^d2jn>sdMj(#-pP&x9p?5E?a7)L8A(L+SgA|NY5pC9;TU2p~z)$!y>MGV)}2Rd?&4C^W;@T1WR|4x=B4=&_DX7W|wV-KQxPsAN&T zb+X3oOv2-AvFwl8c_`W6Gmf67kC#m9RTl_W@i`EVgpY z3{O03SbF5iYMsY75bd&{!F5Cv9;+tT7;f`aiBOM&g)Nm|YpfA1dCp`0-ozE}?>$2W z8uj28<&i);m6>`z_OK;>n%g<85E&h%wFzsf?PVtv*>{E$dYkA7=V5Eu80e?C7g^X% z81>zr)5YMDMB%;6NB*b@sL}`jm7a3J0+RkSig=LKoC~s+E}y+v$yvQOH^~J7n)euBHQvzW36rl) zsBb2p^ijn+YX_z}#eYb!WFn>H*&<0$mD_Vg&T6HKt1^EM5g+F7Xrzib^SS}2x|MJf zPm9`9cL(>?fx)CU{QivYNWfx}!SzeNKF~wa+}VNzOL6M6S}XCb@N*Jr`Q`*8CoQhR z?QGJ!Y7g2f>XD7s#h8O|)-kWq6jT)6f!I0TlvWM;zaXOaKOtftuSS&xEoA9dj%-r3 z(%lD3b;X5kG=DJ+VLN;WLD&u{Zc5)vCqCz1ST^2Hs7#H4=~@@Eqp#1C6&Sg*=XU-Y zLq(U-pEy`KIUU%%4|wcsX7}4`jTaPuM$cuv*nk39v^wXvIWoIN(#iu z^FUQKAPToRqj;jw!p;j8H!Zq*h`~&1xQj*(2Is+z)T*YOc|u1=6YQ$=({B7)`XC)D z#62+C3}rPwhhHc)wLg8!E=!KYaV4y@p*^xz7#+sfop0}5&+#VmTO@1jR(HjD(bF^d zHcoCDTh&|Sjvm=;-dl|urJz#^vD=)=)WOr_p=!O1gdd@Ny?t+XjaW1|eki96gOFZ( z!3bWxpEGL>dqvC_*%~%naXxd`S}`bCoWn&tLUiu2ENr^M#c?1T`(^R(ihWz?pJSI^ zh~E980q?!7s9Mtu(`mXlJoQ=XoUCxQuNLOCM>Q#L%)?tB!QnI@DO?q>Xdt^{>mv((ddejPyb4h?~2;g>8X2 zNFX7kUh21EzXn4Z=%4`q6WYx<+3~&0>o+@K7%H3?B6}=6()!V2(cqn|@ixFgBu$Do z8B|o>5n8EiaIS29(cpmP4lO(qpJ&)_JNgY4ef2&5if~zu{8$OVn zRgIYiT7H~2|A{L)?nlJZ@fl%0X3b^js$RN%IKmisKbX;!%W9}7hR(<*?NTx0JPbc0 zLusaAHt1M!GkTW|ZwM^gW-x}%MLsnkJ&eQNx9?5zczd|R>hV%0>B_J*y1gOZ_m1n5 zep-~rK6TfM&$DLp;?~n(zTw*;jb&~3AIM}uN)QKMLJ!kKPHCx}S zL3no&N)*s1^1^ca_%5&_$5uT4Yr|oR4=ac9TgdQ&c`wBiOIAjsmvRi`a)`5Quk=Cf zGXG}#GhoIFbcR%$qeZml5d0s^y=72bU$?KDgaAn(kl;@6;O-WJI|O%k3pB1F1Pd12 zJ-E9=aBtjQ8+V6>v&jG5wUc+>TW9ZkPu0EOfC8#(^<1mx9MAYY18D63gu7PC7BeH} z@D7O0Er`Zj$*c5MnS|74=u3@D#CS5RF{l{y4tt%tWuY!fMUH@+H^Z}qnoj8XW z8eB-7Z=YQ85bQaHqBCoofEvd`|D;I)JGR%`hQ2V${jNCK+TbP0qrKV0-5rWjD}^NY zBc1Q?Mes-&MqqK2A(Kp4mYmb+@NY&!Cwl7}E+2j(37-Ry#|^-N+`n<4xsJA2a|3c! zb#4J^m*z@eD5EV||JvTe;zs7=5R(B|GkaEBzqdr3377s$-qQsFuY_%%LQ9te-s+xT ze|Ni2Y@IAmqA-_ZfG58@x+oT8+g`WdeS*<$+L~_RZl>@eTFvziyD6ZSddhMzo2Ump z$nb7~)i9=dDc@3d7FZG}MO~5jIWScvC z5EME&=#A`9CcO}HMvETFQRrF4h-6_9uT_k(DXcYcIqOQ7K8}H&TXd^Er(al||GvxI z;BLH&DJUU5$M$opI|ixW*q&iMq9v_i|N31Sv@oUVy8}GaPwl^!h$3~7l3YMBCT?zi zW_tJoJHGvEmSG28ug?}M<**N%+iVHz&nM-MeM=$Yx&_(KoukxYR9HMMk843$A^rpSyb2^v8c)RezU zHGeLi7b~O+E6A>`mPI&duXQJ^eCaWe>}m1N)k~#TKBy3V%MFnl+_HTj_0v40Upr$Z zoSkR%ewjI|SG?*vi`3%pCR$T6>)H_wWb_q+rzdu_P8(xyP<|vA+Q(Yp2HNkI zO|!`rm5%KQaUO9!TR_}frma0tb8TSKaO+t?tzeoctM2wAMAYwA^x=PDdD&rgwz z;Giis#H+V}atzK#0Gsv!hoa;m`pnrjty#3e*wXa&?Wv}b4H!Wq)8=nT1QMy%tx6hU zsPkD%ax2AkB(|y!Rg_4{tN2o;ht`$FI$h$a2)i>{pd7CE$+t?IS4N7SN}rN>W8de& z-4Hq{Y}^mW1Cbfg=oN{E>Q5_#ui z5}!H#c+G7z-&;g|fMD{$1iR(lwzUo(PpOuGxwT1@apxd$d%TR1&oKNB!pG)S8+;DW zL}$}K9hPKHjNps!YcFJlzw7cXEin3t(fyQ2_U(LTV+N8kdzTJ3`BX$_W8*{)qssaVlecUI}X0AW`pV2z41%veokcMV4r@V zzi29rMC6S6EAX2Ohw*yIT}30LN?srl{GG;uS{dOGSz+h+kl$;G8oSj z-%1@Wje^kNp>ODVC()EHcQRc<<4J;-+l>tN$FO2O7Aus1mQ;%BJr@0#0Z6jVqX88p zfbUpuLY&#wFq+{lqdve_O{WG<2sqt2Cb2aY!>cd!Oj}h|+E_@^_DXC!n&TerfuQt` z1Of>?eH~*M{RzlqeB-@4)*PWnFg-HHfp6euq|Qp+agLh%)H7n$qm*s08& zvOEQV(?|Qs4)BwCN+jxF#{P+UXeVfXlGKapfhZ`3`T3LNU_$M+K=um*rQbE(N-E3^ zq%<^taJ5;TOZw-#PlE{l*GA^TSF0+u=V4PWJNrJ@R(m5s*cJptM?T%oYl)!iI(M>E z$=oCZh4cIF;pV9{5B3vxZ8A^1$#hf?#q@m5RH}{gQHSqOr%ZFTkd-T?q*Gv`#M7lu z(fTaw&EF&Q-TT9tAiemru_{x_gCz6TW=gV!hQ@9ZeHjW zO<#*A&DI5ty4rtYFVb}xNz4cUzh1kERUgd=3~VZc)fnB8ADZgAp2m{Fr{-O6;L=wb zy7M{Y4^5k52Arm+T7|r1760xu&HC&&*@g@6TK2T);sIj-s>@$mHQ(^sSp8yGSj_yn z&VEgUZ;=b?Xcp?A!c^&2kytrRt34KH%!mz-;4gm2@!->QHL|l{4oCXi0IfhX!!_9g zA^_~UVa(Xd9!W>BdI46R^ubOC_p^_rn3eBl!%}`7K?up$?qjFl*eIxr$liv?9&&$; zrTn1{N};W7RsPyneYBDfpRy7zUwRoCrs_cYdgoGi9OSb-J?lT=MwdH5Wc=~aD>%?& z=w$3`4aXa=w~;n)LK4+DHO?wILvElZ43Ik9QsF|Q{RfZU_)|_*9F|syeQVnD#urTw z*aU>p{TH2*s^<{lXY~uMweTsr>XUdFcX8i}D|P-uXu6dG2u;`e!@xgcjM`Um12DYk z9=aZdrh@+PsVjtl(3Ix>%VDTRB^FamcnN>d(q5f=oyxJT8;U<1Q2N<@j1#=dydFC* z%ZYcDcT)&e334him@(KxcV7m z3v;1TdT=J`>Of&KrF?X2awORE#;lP!b#-hXb2#}indftNcT5M~xEixQTlmKR@XqYA zX`t)579|^EfncX+W}Bl5(W5h%_O~wBHHy%9(70cz9f)05;iWvlS%hsO8(!4zs`5`P`9NkI6H;Y%A^GDheoNvWSFRbP~IzR!CQ<#%ebasR1ZSkuA`trmkS4! z8~$G&%>Jm@$JnApUF*KH)rzx{x=873uW66Y{lNd`QAM_JM{~L?+6U9bb|K`o>Sf5yzp*?yqo?0r8`38I ztTNTLXx*(xN?17TTvRbHvf6U7c(FalN0c4xHhoj4iUjqH(|pHu^4$H1$%o>?ctHVI zTm{N9pYSU@nU@?Ufr zn4ROaA8Zc}-2&CcKt-{~fZkU9jR2W4x`>Bah>iZTm3If7Yq-%8c?OJp%UrFwR5#N_ zlX+Rb$sPqUaiW=r>*(s5s?54~o&&wBpOLNZ3}3nZ2d5#n!Jr%8!)EHZr@NPt05R;% zEo-`70-yhc;~KF^;_9%OLt2MS*#ZeYePS)wtBUPz``;2s`{Jd~!v`|I6MCdIj|n{* z|KO`JLpaTM$kg#KbHq91FBl?U?fzcM@&_DFmY*!g>`p#zTx|@vBxl}Hv7uUWRz5E0 z4ZH@q=v%^Q=x_G&*}S}oDrJcKEq~~)TvOPyeU~GR|L6rktmmWkT8h4L2@;qxW%Jz? zh_>iT;DUc2`!O*ei12mAw|m(R@4m%Jf zlV0P0Jz^I~XhY(tAQ*hWAh_QYC_bHhNBccmwwQRbn4Wp!vdxh#7O7%!d9aSWd=;sD zKk}j6+@NhE-lTv;8MMmbiO1#7+AZ}d{+#UZ4Wnn zNIko>(P0hhp8tu=0Uq_%|%B}8_WTGO}I2r;3_Za*IAGG=oVh>FwzaMk} z0?w3kjp-@~3noIU*iSR&~(2opoP)`|Q~49OeTjf{Rxu6ahmj$0x6(^_t1C zx$q`WN<-1F&gwlat@z%2>}QwiibiIA$ixP^UGiO0Q5hT$nT4KmWDyUkLBl3`pCq>FT<|r&Sa8i48tSi+522w7znMVU@ zs7^F*33a><<*S#L&0mMe6v1WvpYrZSjatxVKT@)dJ7x7Hg`|E|+Skaxy;00cm1W<- zkzR3?+ejz_w4zAMpXwLbovpaoSft2{Tu(BRtdvYN#%Z~>;U_D0K1j#H9G$h!!O&^< z6sMK+k{qnL45(QV^N#dQ2vRq+exJ%Txd%g-H_O?6vL=g$7`Pp|yn*EjAEi z+BckD`rWS;Tn9^+w0JzLyd)$0h2F$mvPlCry)KEAQGEVx2UFg%c(ThwBdrqLI=x?m zh#r(lZ$+C8M&_f1a(4fbiKacuM1`uG&d7K5CFk=>wcl|fGf?iSWX)yZp^t;EX-$58 z77lDjp$hN6_(vuxFXx0*uz-zT+1a^}cn!S^Im5&1y6hgD=gGx}OwtqAuhS?TEM}{> zjxWghdXj|kP@86d#Y8t~&;1X}EE&)&;GF5>gx$l3lpM?mkxi9*vAjtxrgnzL3XKewJ!zJfN& zPno{gM5dyP)S**JfyuQ_&wxeiFPtl#>BEXv!FQ2gtp1!q`^~MVmzW##>$9D7+cL$% zZA!|Y3Sqzev`wp}KA}7v1ioDS5h3BXnr4(UY0$rVSEdRk3?kdSeBQ{P*l)F>QnvDt zAu$k>589sB*}EI^GIMF?0aAF~u`RK2TJt@stS)N`$aXR&bvwbzf4<=z=-JZ)MY`;Q z1-uR3g~FH3{S9uAKQSEspBN7HPYib-X$O*Q<8noL=!`9#ai;Qe#6$Lq0G;){&Q**M z1e4(p4#m3SIEEoBM>f$-eaMtD zX<90`h-P^p?yZ*W@N9??-zP@;i}w_T&=*z^@ssRniNT++ih>$ZIzukq`kZ?E-AY=5 z48j}y2dE$gE=Po}n<>WCf)hlZ!1hta)nV9k1I0p&;v8U`Q7RM-4G@PY4r9!Cm0stJ zoniBRN$KX@Mn`K8Tk4Ks%#2Q~%Hx$(C_calW+jhtws?I8NEiP__$j3USNGa*JXU`% z5hkI#yA}^&Y$7C7&sMcZKsy(&g-R<0!$~`;v)5+wlC$Klheq0uETnD+)GxctaVaVWz8U4%4 zot~hRY6q96(bcsPX3y>GLhh#$fzJ}xd#Suu_1+-Jxb4<>+3OTig`Ha|O2#_}!27A4 zoL6Iw84q|rlegWgMA~W?4biAwuWvWzUH1^uGUtA+Zv_b@C|{VkVc``35A_8&uqE%l zCo}usM`%bQZqJZez3@fdSH9(cI1h^Ve)5VcsOiE-X9j2}cEE zGt5pDc^B+gXHqhRvaM*Ixb1LRcJIf+bVJLG<|r*)jyv$HM%OOF&-iZ;yvU3*udB#< zDsbSnw`MxJlY|>A-jVoamC{!cv}UhaF500)-_G8|?_17dgU9nKYNsyn-DDGA9x#28 zLm-V+owUVw9o1_yEUvTvu@wh0ROiiGq71%?(FA9s^H|eezSxZs8a>^)im!CkG(;{28$^ z5r@<+%FMyyEnVT&KSUq#7qM;X(M!eZK_5|EtmOJ}i?cWpc^Bs(a< zxK}?*cgT}1;Wb@qq3|}TMt!@#to*Htz^Yp&%PRmgRz|d9ZNj&3-ZQ;p=OjSun9)M; zF!K<#V%v!_zVww zB6A4wMP&?hQTcg#h)(xjhPqdODVfSa-)oRLnT2gkaD{KDINJ!F@p*YP(j(5@aFgnIrUlF+ogcJw0&`4Jc>fr}Vicn1E*>-WwWs(Iv-Y8BZ!KTE5eZ zUbv*9%3plQp0DXIa{1U170zLQ$zf04RZe`plxXqd>h?)?BTi^cbjLApaY3gqtEL(9 z2ZZOz9(Pd!o!y$zYq1|)qD~|!%+7D!-`Mjz`2m8o9On!Zop-DU3~zJFPciELe-&gB94=CKn1Ea73+gg#&ivl_S2GVazVkZT0_Jo02^sNj9mNHz{@aH2^U~CM8 z0thj7VIvHX$~Nohudkgbz|6@ZN+s3vo?i4)+?Zuh!@=sfKe0qWy`%*;S_F{B(~qpt zPHC#z3WFulVQrfBpLMAZA?NhIZgF0K{#6x(P#}CQ0f;nH^7_hV#3Ws zTK8RVxP4ziJFi(4qgE_yA!HRSDDR|tGicbV8A2xbFnAa(W6mrsGPDvent*cN*K%dr z&dC`%!#t7Gzjo?ey2PnG;G8^o{9i4c>$*S^YVcqw0!A|k;5n@o7BibQ!($;&!@1BVB!9nL^n(wpashq$9xg=J2YlnR zZB4b+11SE*a=2@90pHgF%jV&U{H{qnqY0gY6@A(I6luH}=CAQD*1-IwGXdplo=XA^ zL*0VEuCloyB~D>JI$=eAd@WDzglDNm`X?C8|3q}A4m^wHG0d>TwH7vh=M6z{fL}8{ zk5mZSHA>M;&Qw%N{EuHVPF6?X)B?nbupVCOPH>8mQ&*{KokiD`Zr(u8rOpn|I^N0V zz&={@D7d+q=;A00F>xuKnz0#&DXf;+;0?%af`M%7_`w?4aUPPQEbh5Xhh zD(|s?x#NiwRdNK6h_1eT;A2#Kh#)3QKMEeG2#|_rAA@Fo(im5=juJ<+N<6AddbJMuGq=zcZ()QhFN@gY#jwNU)rN;Oo!kX( zybUuoi^jHMdc|5T!s6jh4Qq!tF&*v-eb-E`gGyJMLsR5wVqP>I~B73H== zT%k+4!Ix*XLVY7JmTHZ`sU-W=kcF>{Ua4)v-YO$K6tPipVxCZ4Y+NKmG#Uf)+ROJM z3m(kLTx?zbG!YN;-J8gcU2rD$~WWhDY^!`ct*sK1N@*Rb(P>cT`pnUn2|GK0Wli!W`diT_- zzw`PdVl}I(7!*pF8w5{uF(JsinYMahu@}KWRPKW4C6FwIGh6YIkGI;>J7^!i`TD=J zoNt;xzwt4Ybqm@L{0^baCO=EDrBESz=-W|gem8;l@iGt<3mQQ|i3-d&`D8zQw{>&H zJU_L|VsXDjy(fK^)vgtZeR?BRIHsd#(D=Gk@BCz0ADE7)l^_f2!nSp4uBemzR8B;D`U)mT~F3-{fD5?g|x|xJdOSP>}_Em9cuko zyTV~?*n1Dl44l4R<9Ayu(6?QPrh;o-p3bx7Vq7S_n_(@$hFUQxepditr}JkG#aO1W zhdYfjT~a8~$tf^!kUUh%AckMf){iIAWNlu3HA|9xgx>n>L)A@7iY;#-w3qYO=n5p zc=5|x&X{%3#1C;VC5foyLB4YB*$)Yc_&Q|?!z5S6d01+dC$YYTOst8n4*%e=SA%}& z1a@H!oK5#^zAH6OKf{N}A~I2d;fO#ZDIo70Dq?nlUNcnl-(cww(99#C4pOwVf!1Uz zu8LuWTe8b33?jc2<|#HzZ=-Up{#ab)ZWM(4X^9 zEp;`qllN3Y!}Xic6M!m?+bE77NI<`y(W3ZoJpQCs0Z$prAf}-1HV9H%B!2HR;ZLIu ze)Ghh$iQs5OhzHcT#xF97@GJV!8E(OT+27_4_mvc>8}x z3TeG{Qsa6>EVNrs-~(i=Byzl##2TuNJtfONHtp8t={)J?cL`U*MIH98ow85xI+le) zv}gy|P+59py%}tOkTRaIy*1WU%XkEawA5Zmm6Bx2PQ2Fn33QY!0f*F}(P@ z2db;$*G6kWCtPt1&bqmiMS9!Q;{ORt-1hlD#}eyQ|DUr&x3b(1kxQWmTWlRqd%sE=T#Tg;l~!Hu;QLXg2oqnY9FLu`9dCi72CjxMmbiX-vCNAy2Oh**{JsmOZq`h#)vd)|%Y@jKo z{d7rpf8>rxvl^kdlfX+${(1%g&K@9s|2!fk?E$g zB(Ccs&c~Z-*#Js|f$yGlF@i0Gs{E41YD!c56`GSy}*YNuX&mRZtNZR#t6f$(#^#t%s8~=hmDUFE#obV0R z2$?yr^fwB?+z17AlXtSwQv7akPnzQFE7508ju8ZHa2EAlr^}ybvV7G{AMh1+io%re zD3L(QhfwMcv#MVo7miP1LcbRYmh(crhTq6zyJ)!=hlpWG$AHxfGqjsW$vTT4AQ)cxW{2Gd1NUn!l&4 z{`5Y8?JP-~+Su2bVI2&AopIOInNZ{0SQzFQu5D+9?`_Cm@I7(eQHv;NQGb+zDsLMp zMri_4Q2Zhua0NB7_O^)hwL@Hc`U9QOW&b6!g|L%;^}g=V4ej16C?N=zov^BF|Am#| z+E%$1X)H)^6mK$RaGAOyx4-EZ`-`G)26MH*xCQ3Dsb>pQ9PA}zmK&)s@mSKxs3 z`+7&}u=@v_gq%J7je~q?*^SZ1>4qgf;&J|!*?HJZkJAlFBkbifU49VaZ2z9Xsao`V zyiFgetyoC6o&$bE|L^g#ZAy=2-Ck`VJqXN_*>lnAy_1j^)2%=lGE>4*=9kbY(2RfG zA4HXZV8IgIOt+A`i+(zre6 z6IsGW-sM%_y;nE8Jga;u-ANPVqo}})!paliL4n(`q}|}3eapB0;9>tQgLC&Z^z@Wk z@?|`CO1Da=&=3qQMWiJ%DkrW&o$14utbwI#_b$yzzsl^>+>u~s3nZQkCjRcItzRJn z3Uk8cbYEg^c%#E!{oU-FqxPNe3mx9Hx?RlVv-%YN3X4t{0-DOEb2uFGeR5(6Thgzz zUK@Rceu+3^Hj4_uR*nIO{8#M$t4t%$8W>KpUOwk=&kUtSW{J$CjjjkT=5`^e?{FO; zA5iU8sGr-eMJl3Tt60BpD%^D-VMnZuPp9`~UhQ$BNoa`Jvd6c_}FDc+Z&~T>eUWduZkNmh1S-( zF}I|)gr~r?U)|TnC&II?&EE(M#U|9y+T4%Oke6D8G4nFw=CQVR6PHKCci}S zCtKWF)`BY!6QnR0s6&a#eT*WvMgOkn2mF)l;-Y__qBC8(ty|7v%CC|#hG7?_BQg<2 zyupO2AfwH(a2ZpwdS1JX^V+u=3@iblJoW>N+Qf^>3r}(3qcFnz7_}O`ifBfr!6VOs z*6Li#gBS8JcI7=_-?T5TrkXU-Caw~%w#P|I(=iihBcp^KrJ?EUKYvE7jDN9uomwDz zbhMj!G43dpTV<;1sEPi69Z7n-EFf*A3n91-;zpvR@bdhb?f@@cAHDK3>sW zmDwP>TP_)Eo$eiF!T7o>f-b~`E(>ZDU?w36-AqRaw3xzlzl({GA|BH%3U$)+TlFca zi-KjA@y7G~blScWNTWenQt8E7elby8#tEUr_c}`a23P~=F4nP<3tUQvo*B?D^Yw<2 zN*Q5l?AQ-OHKWh7<0l9RqVDle-;J5wAQFBUT1z>5^?jxL(4aqVuzzrNJSB@##R;Jc z2UW7M3hw4L%ucELZ*^l7xvlEyV~S;C{YMZoUo%}wSpVQ}bLjfJ->nC9vVnJB?MD?H z+!NTzepR;Yfy5ry3hny4mi!9OzDz}9bfsN4`VtW$*^qh>(1)xN<~uIoEZTf51Bu%M zVBaLS<2KP}xzH5X4G(#+EW2%3aaqyTh{a@}auQm|d^^0scB5mndQ`J}&71{CLQJ2c z(4DT(&E^~-m3PRT)p0aM8KU9lkiA&1Ww?`e?Ua2wf%9^y>xX>dR_LXqg)>RyW*Cv~ zX^YG4lAAcSlddHJW+ZCe)<~3w_6rIYE6l@(gw?AcLNiA4u0MlM=H*l@68}L6A8tUbxT>$Vl#z zX8@7$1~y% zme2~MOmG~193to2q?y%%!DgiK^~gJa!G>>g8WWi-HISY<1I|tcCRuU?xmhihT~@{~ z)4{x``d}Amgg=!|fln^AnIH)t7|JP{JrQ}6+?H$8yJ4x`N)iDIE43CUtj z`&G#&iI?f06s79;Ysl(JF18|5!=GO9%AUjlE>Wms1Y!Y&H!UvEz|9Mfie~fZ5)Iwq zG=m3RqJ4$E%yslmWDRRlEQIzH?QP(4qYIfFn~#!@&IJS&Rp@ypE?R->iOggnPM^W$ zeI$HUwB=&zw}~43*DO>{B*ksjJ!|90y8+D>uOf{Fz|~GOF4=`9Sq&LUl)@5ZU&4!w7~$--Tpc zIBs)V$&+#P(EIFmGg1JP)xUe#5JmH_?%@oM9ORxk_-WSEp|# zp58owUX95n%)EisGoTnvhSEUuITbAkfk4KGI7H&u*rb~Yq0p{6|&@X z)j6pel}$}n>2B53v8OYWPtMRf#*k%1#?VH~&$*@h+%zOsdZ`podpyn%Z|S?Ecm4wv z6dWfr&ziMUl}it~dMz-I%y;k+Q-@=@jgiTJiIPxk&~$rg_oZ0n^o&g zCGlrAbAG2?9rQDaW1t?UC5>l(=&1U$+|Z41#*$aO6Eiup@g3s5JmF~Afx+Nd>haj3 z#5`IwN1;33Y;?}eZg-mM2Jr}R3gV``WFU4YW+uPZ2M>}0Jg`Alc{95{Q*}`1qwIXw z?0IIbt18BBbHKZbfn>)>wuuT(w<87T*2c~qX;zo-h2v`QcoQ{Xl?USt{pzk z!oL>t!tb7r@9SPR#YD@8FmZt0SX~*VyG5rBJV-IF-=52aX&XDwxHJ76oe%`{#=hE+ z@1sxtIdOO@Fbti?zGo*$UJXp~@E)&i7fR{_>vUv6Dt)PX=t*`&eH8!^#Btp>6j7Ia zpRy^_P+&d7{a(-Vz8b4kx_V{_2l#LEi7nh8_-bu|ChK}P$TsAKn-Nt(#`BD0^D zcNZs{%)=S8|3LycKpda5@`z!kQ+tZ@Va$!@D5c}uORu%?z#sPzg65QpD0~x1q;C$T zI@#}LwNZn!ERIfKe-)Vwr0_Hx+l1lY&!z}+tT!Gzm6t5+7?c8i*IzHy|tC6vwz z^gr=EM3B?XP;~}knSe=xKL{b7owH((%yLlSf5R-%(LQZ9kuKcB7ai#|ja#@(n?|6< zJv69bl6&2g?oWZ&u8|ESmwqt7g66m%j4k^<5+v7*{SYObG*<62U&Og11JSzHc&S)&7K(2g5)m zpV!}nz)WJC`e8NmHDsN`ct|moE^KJ6)f1x1xaPptExG^ao%k)60OK}ZJqWH${}MDp zQ^d8U-r8bRGVuQrRBF$OY1DpI2WG*W2l0UK54HHrp)|~!14jA#WHEOMq!oxTaX7>K z$2swQJ$*5dMS~zj%fD!td#|aj=^KlCe%F;DGWgnO3F3lN4!508>A5ZrLl1j( z0yxZa?=i@ zheYq1XOmV2WL7pWp%pAR;izL@X^pg%8)UU({nI8du0!vh32ClGh$I}69Z-$A}%aXsqq(pzg8}Z*GT@n`P zXc?H`MRl36bp z1mXTpIs8?BS&MrzdX+;pX|Os+oKof$T9IR>CTk^1 z$x9+3T0?V@uv-eobyqL!?vm@_&Cr)pSQAJf^rfEvA3usKnt`vF9Zx;@oeMKw+y+#Wg?Yul}!A6lcj-1`d;)YTM8!aol1v#0K84l9`JH#M z5%2~qE=e-Kwc8D;>h9OtcUUNcqfInux=#4PdiNa- z9?1~dT#+XJq&BP4P?k8}EH%FWff|3YAwXSa6DPF~#%yvw56y>j6xPq8`-2&(@H;B6 zrz$Q9;R7U#>YD6^=JSvoDCzfbpprM%P-Ho@vXu=(n?A}~229H`_ zty68o>(Fr&WVRDB`vJqQutV_zI2&&K zdHi*55+YMgyZWWf(!f9cf!W&{hNa1Lm^y1w5l#WY^~^$iGZbBj8$>@ABYtu$#lX6p zZ|TGTt%N1HGWDUoML+Qerc;t+@vNoMZs}xxx+`5!$BL0rXS&8@7&RbGwI0u|iord{ zrJ85@ElrK-)U#TTsZ)5pqo_TdS8%JOhBBm5I_un+8)Af!Vg5f#{CO_aoyTdMLA+}Y zerBa;bp296PC!a-ZD;<-dATc6O{d;H({W0gtu)3+{$E0V!CdYfsDEAsV2GF;LrRry zX}&Utg+=S3>&i13Cg1Z6(L}@xug`HwZT#k*_6hed4B#r$Z4LN&tkg*H0}W5RbxQSY zsK5i~X|U(|qjQnlBN=>r7}S_D-EXe1fw6X|-DFL=8e;_dLQBdl)@Wk9veet%nbXI| zIfaWUQT%hkZA?HS*VyCKi3^=)l+J04n}^GWqOS8*W-09x5KM91t@a}>Ga|7$3o;{3 zMP z(W6^bZ5TlP+v66H<6OWKd$o}-L@`F8fPd3!S&vu0->T`rgC0sp~py>70 zCp=dmN@z5H+&dO*Zt(Ez*0!HtAMXj*wN6u;LCJo1zA9K`L(NR#^|<4cERM5k8Lesr z-_uz0hR<`&Qps_Zt7O)9*GdFl(6pvdC>-eF#{Y%e?OSaPP}gD%?oNU-BT`5$MwQFk zQ)_Xah7zXA4Rf*tU9dOe)jDA zoy89}$-`q9&L7k(QjMQ56A}PvS5NGJQ(?yUN6xmAVR@K2WcgMEJ6e5+F0@`~a;>BP zzP$41)cUrR$G-T-hteok`0?1Yk<-ZOiXD8%RB+a}H(Vi!t--Kwe;V8c~rM;kzIb}uk;qlt`(Av|q`xH89sN}&^9bXNn+k9(R`H^H+;_yn!D?c zQ>lzI(lN=P#*h7|^+f$@LzCa(%?&Wb?kw5fF33_f!7n{4r3`sGH;P%{%yn7ZlfSoX z5#8qW`eg{xS~m0bM9vHC%5Sm;K^E>RA?IYas@)uxzfR->m7)hsiO$^CUv6rrijH%8 znK&f)vcZVhf>OXrElarI?VkDz4n`Vm4&8Br@=QJ?Y;r48yl-slQ>7v&K- zE79}$b*<$3dQW9eFBjWl`?+RCmSxr=eLNgH+8yD62D0P5=0)`W?$c()8zrBEz^H>3 zo}4;oh$beQzBbuVc8u26$O=m$W)bnV-}O9yS=_m7lSEg*^?ZXaG2_Ssd8_%rdE;!e z;*R7M9!SGgXUsdBXgJ4 zC>;RU3a5g~PW@5PP6>u+7p06hOix?;NY$_8$QLKS*G>^`L`sJ$$%m2}Y(vRUyAVm; zp>W{*m~?%u50rWG8i9*i?e!|JHVUg6V~%So$u@ktUA&~J*p?yFiZsdEdTq&q*N#h< z55I;*f(g(?R!<wY#kW@Th)WiO)kW3sV zRHTo{bKdsXVzbP1&$-E%Pbi4bL&3drbZGJgQA%eP~_E>&b`rD}t z?T*G*fhHse@LGqGc{){C_OgZwfZ~(|b2n)>;^RQU=1h#_&58jYqF9=AZxcIwx}vJ%7K{d`AE&p5(Zjp~rk2?pFtc&m}lpBhFepUF7SJ zD)IKETGsP(3u)6he%h}?Z)267HMPBD6ew2dee%I)KI7L9rJ0(=GYhy5((fNFXUVp=}``hJ>DR-85630`IdSP911uI|BpJ$yn zZ3q}SRjMPUvX&Fno~_qQ6EThs5(>$6Q*YoZ`-vR;=`lThH5`m6@op$1U$P6M=*@gg z>1LmfZ*yZlY1XK-mZMPrW*TJ}yG)oCj=rqo(K>Ug2Idqt4j1ts)@8A?NEQ)3-;X11 z80$TxX~wwtAxHBHV1j!&EjzTO2cvsov!PN^u94p zl*Hb3#&99kW(pNXPdb3!kd(~Yy$*g)7%5Shk0%KM3&oWOeDBYHKsP#jhGhLcYiq^= zYubvox8*}ePr8udq49o7JuUV`nu9radznvsT(?7ZGWLnZoQ+{Bd4(zoF%MP<&to=~ zsc`D|j2hMmj`fUgJ&)uD3o4V$lfsPj*-&{1xk>S?Z{Kx6^_0WJoJ#da7&dm`p_uPi zk5R-LXmV*T&qoC{Pm#YAk|;CRSIZ8lGHk%d7exOjIr1kt=eE(m+yv54HJWqM{K{7)Ue&!mGvRFOBZ4P zFXqlVDz2{0@^KO*B)AjYEx0GR7w!_=AxPm+Ap{7ng}VlKx8UwhA%VgRcPL~k@7G`V zo9>>O?pZTy&7ZYkmE3#Ix#v8;XYbv5=8#t>+!b%%)}EOoQeECE=i@`2X}5wf|3n&I zw*xRwY&XU~g-+N0B?fGmc$ou#UrlXqypf{G^_rxElcLP{_B5t`qi>7Rpfc`C;2*ed zaUFr{T#|@K9}>fYrX}c$pPPW6Qu{*-fqM17GE?+sQyx#^Ao>Uj#C2TERId?>4YK5X z1Cxur+mDbnLCDq4-kTR;2R-pLL%Q5~g60T#-+EK1(W;*hPIt?AeJ8=Yp*|*uINl+o zfd9Oo(^5q0p4rI=DIo9bmQc1sN6nIyuz{-J02_XZ)MWYwOI-S`GVMB)!de!aIcHX_ z#VG^CBEjc?A1GlE(g@MelM-m!vc1k2DzoW$BC@H^K&6FRbZGCrfzGrh{A)*=bh(m{LKL0UOLOG6aWU6TE8l@F@ z|Jm!-sE;K{hCVk;FNF5pvgf}hLfkf!2d}P5HRkkfa&@`i66~I}lS8yd(#9(|P$>Gu zYWhoF)Do+_`zW8mQL?=Tiu}XHx{Inm3+AzrP-`|p^fV+~gF)SVx*+A)>S1-WqQ=1W z#nRZgN!LYj?1U4a_GgtO3m$Lp@}!ek)%RzHV*1NO`$zbf2s6%sv83MqA7Lq> z3w@0WP_6Pcg}ktpc7`JMu~8x#y1}l>_N)j<-A# zqXQ?XJI7UEO1yv622LGjh$;Dg`SVyZTo89mbY0hhDX!;}1ZQhxbO?#c7C{Q7)P zEPJNAfNwU$)f~zyt0zSsZ%-VOTD-LvCR?+E*3tobtn)Ktwa}HI5xsYJfgCG3(tjrV zQMDU^WJ)s=oqJI=@-;ZO(y`ffI5}cq*q<4_118!`smFC98d3zz4%mYWCVxp;@shdt+4 z+MhK#?qCjU{`w-j<0=+)5u0(Z=Jy6IMPw&m)!GUN4RjEBcwp(c${QUl`Y)QoQ|36> znc(4+6Hm0JCmLBqK_0`+$%CWBl=s{C$R+n2O<9D)jHW3AKreB}`L|r9p@w6aPHubP zmt6MWDFh+G1(xid`1g@tnXG9HN{9!uUGJkLMP?8dl6+~Q|Lg_mjak>Zdj*^O+L4;f zlaR@iVB)M-&#*O=>Q!YYfO4q+v&GbUKE{OHXsASP+a-8E#Km2Z6%q3n&r>}2=`Q}S zm_My1&4+F`k_H&MKt_ovIzXCqc z0T$LA^njl_(IQ*Omj$Wd6DGFLhzl=3H2e^n!Pw09zxqfhUwi+h8H`I4vr?jcY9_yh zd;0NYc-V3)y(_My|F=VD8nIW-J|57n@K>qckCe9N{s_p0xKLnr_$Tix9J7^}(apK4 z?aZZDvp!0@d6&^7P;^!lu!LFARp=qWz0RxQuRPS$Uxl1) zJS51nD{}Q(7QIK1EuQTyD7@nThp zcq)XSgN6<8Cdm85T6|$(mi~I&xa(cVME3V^S9UBzTMsVIsvj$4*pBVn`c^6r z*W;wC>On3lL#F=gl*DGE{R^nhO(ANY$aiP2xona;OfiG!uB18qIZ3J`rV8{s3j6}4 zNd;5f%ffm`)uqh5Pz`P!IFm2bVga3(gg|D|?0<=+yzwCNz%&+TyFDC*y}H|%I&gQ# z&{m?RY1Qp7weA0H(3!??EdycOU3`{?SSGhv{BLP%SW<_859|9!yo-E0+yY(^!XDC( zd8Gq`6zD2wEXS<1kz5@toLib8@!W(lZd|{7#)!c_(;1%RV9j&tG{U`Mk0txp9c}$*Z-rfrTuKL&dqIyS+q60oNaO;99j`! z7J1om=;j$>{ANZ4Ke!QL5h=2EG(iFMh(g#&`**F1PvAkz**52;;y*M+UL1(6X&oT3 zd?kQnylGR8TSd{V6*Jzr;=U@FNexZpQo%i47v2BvxdUjfx*jUq63FYA3T=&G-tb*D z*YtiUvXKQ(y!~UU2{T{0R0e}}fW>aQc!YD2;Zv<2N34#}dBBGi`_nGpo4v@Dk~h zUk^`woJj-&!gFbOy=$K_aeNh;q?u$A4ghth7$AuaC{D2ib;=@!)6sK`K#?hRs5S1y zvcI+RA=<|hG-R^ftQ3_8&lp!e;qJSTc-Gkq0fx5GSTN|Z-0;rBHs&IC&%j0gL%U1j z9R1%fa~6r3f?bJj1^84L*h@KcD$L0-%=Q?E_-uf;^uwbTNz{y;{=@GItVh<*S+MgI zwJ!+=zOB|x3UqxfVGRteC%rzZc)*C!VG|=a)uh>!%4>NLfbu8nQPR>tvvV2sq97Ub zCv@>fM1?c(lW($4(cQV4`K=N+IHQQ8uz_)CXHUY`i`kcu?y{N|SpkOnXRsw)cPqcJ zv9tpBUEnj_-((9h&v`X5>|0VZ+pgw*(mtZhHB9HhlP6xoAlYN-H_b;@$}5 zUuy8U)tgc&fsmdoHiS_q;>F+ZCBh5S9#_r|n5Xd|KfA|J&(}g8qw~ih;g2t* zBsbEw{n;m1JzrdfWB3@U`1kjp(gOFFsBM<=$?H#BC-v>_5Rx{;$s-4!(6=-uYo`fF9?77uTIoe zVwIPIW<%=%Qf(orF)uJaY!$e&-UNSY82xvi;(5EEF`zC$_uR2YT_wMt@!F2psSHik zCjv((TC!DVm>;tLaCh2}dDQ`Z_<3LDwpp~n*ji-mr||NJJU*8hZc$1RVonu$5Zzc^ zyA3Z!6&(@`s-?ErBbxL4%ckhGRT{(6`;H!$FblA`iCW97&mE(}Rx0fv$6w13VRT+K z`%*+Rdd9(`WzT9U9W6k7+(oNv<&OvFH6RM!ij5aV!`Fj9F4Sb1BL?+C-Lu0XYyv%4 zCs20pvZEHE#XzX!O9ONK)-NC%?Yhu@;89*GH3#Dg2QN`yeXPLJt4&)|DDL z&>fu}A=V9y=TBmK-l4p~y+tz;E?t_mS?yTlaxb}&t}B$wHGa`U=G+&t8j^sqfx6Ss zbp3f*>4|)%?p-epe$nm7P0rh~TEP+#5jq%AxIZ+OyU)j3BOq+Hn|=#LnuxE7^|g1E z7eTZVaFrjp;C8Lhmf67#3GXso;w1-h6t!RhJ#zLt8t! z=dmg-H_Etikc%Ga^N@Z`Hf(nGr|s=6vTEvLA^)&F@K&^Jm9FkI-O7XbkqXj_*N0N8 zBhy`;>~>ZRlrS-}grv!;t|Q64nw%K;{>rtA5#d0L!spUV7ahoZn`&ld-dVA=qvoWw zF`B`AGJG{$A=Uh_ICKP7^Q!#o_EUs(_Vm#}6a?#Y7W`v+KI@{97*AZ@g!jw^a`yKJse!l)2~d-YhD%QaiO+6sC$@sk2b~&k@YUdJ+3l)f9?*dG`|CrR$-5d zYqQ>wct?989@TDeEZSUBu~4aNN8O7ay{|)8(LR=BN?0Lho>BIz-DvyM}Z(bYG zl1rS5cs&esj5vS>f=MfcZi+o_rHBX+RZn%HU%cvS&m~x|xG!0c^OmcT`6ShEEr-nd zTt=!MR}eTupXht%5h|ezcjO~6gm;K_EwLP*g>tAbwua(?R*F@e4qq`|9qE%(eC3HJ z8BIeM{zH_Wop!h4rTuLii?X)@YH(O#r=e~NKl!VGpY0`ZPMEf1T@q6#D=TA}iMa8=@7H)7&gkI4GE;EWn^wMEE)V>w;iZJ_qipz#JYlpSlpP z+!To2rv2|(SEzw}b$g^Mw0lsjxQ zU0VC_#zGdoVD6KVDmE;O)w2Y?n4nPssr!_z`4lS^ne46mKKLaG*-xyVU)=!S7q~@A z^SB?+QXVML<$dQamsdk58pdqisicv}9*%rOOp6TlfP|ow)0b6tJl))b_Z_}dMso6< zmP^Y61#740Q#M_9j6OF6HsgeIN3#HFVV&Sycfzj|L@^jI-#L!N2=L{G{=jCqUW+#p zfv*hNo{}2tj~m5#rv{Ze>mmamTdc-A-A#SmHYi)0be-sDsx-7D2#E9O5sJ)K9(gtn z^QF1Iqa^gyXdd38;ou8R9$D8&CwCji5DH@|J=Wu#iSIHbh9aJJzb!J~XZw#GM$sgA zdXp#}-lk4l;w+%zbq{+NQn9~Nmo>`bRdz(vK-~%5`IKcGEunMs8ur2R43T{Kq-9PL zHl-_avUONR)-6*+nE(Dfh~_1uSIT8J@>t6c6iA|<7yQ6(BsO5odzV6ZQiF>%Ymco;~q*oYB@FywX{SK52(G zw^3sg9y!_2$0?Mtu8w4SH6407w&2kr+Jt0@xOA^GFp7|?TB~Pf-+QRy1r4SfPn+kD z`_`x1oTbb(1sIp<6z^WcgTfJLzWk%J8+X*YBP)<(D8o*iNV6t_oOsN;N?{P%3OyYq zE&|;-W5~t}vzy*1BEsx2c@6c?2yw&4V~?jAuzXHv zx4ZOL=YslHjKeltk4!&6KAW}ri(@gCSx!`x?x`3`#DC)KH+UFK zhVi-X7Y)diGg}1IxQ?>r&A4D0t6h84lwzG^FI+A-N(+01`aOPy1wf^Sj=jy|9SR-a zwD#4$CJk z7hV!xX1w<$3vZXFo;5xX$&1Uw+RbN*r*>F-cD94#n<$rC`S;_(OEP-R6iG+U{aXO> z9;Oa(q-?Q}SDo+lC}nBU`sLGwjWukT{}za^6gmD`YwLJ9ZeR$K7h zS4wobe_%3+aL8239-)u*As;!VmFf7WIRy`qhrP~hw^*Oy3k%+bu!+0m$E;|%NaoX{ zP1Q4ZD!=wVBJFk((%F}2}Dne!0vMOzan-4LcC4t&A$S;?`n*K{>qSF~(a? zM;5ZOmL8IdkebO@OS7Ym%zQrL1Ose$v$%%zD0VFRO=ep|Pm5B)`A(EvG5yAfFX;s+ zHM-UPuvRXiU$a4grzAuT)>Pw-8%YgfHGPX6ul7*Xz5SeM5leQwYL>Meq8$zL{T?M6 z)VDmmAPCe+EBpwXatSa-VrPqh$?>zV&(^N=3CWvPNQk+y61e|Xnwe^r{Z%2fqIBpJ zkLbMNbDjOybuD{Denhth1$?SuTLvqvE3@%`VPp5x`V|F4K~%4724%@XH1D54O8syI zRJ!HK0lpbv-xU)7YfKc>?s|~eD8A?h=v}C#1Id`N`P`AXj=OzBQqiAYBRqJKx}Q^# zjIupm>N!f*ILKLQ1s#J9i9UJ{dt}@`TWLKbvyC76HC_`lKKPM=DW_{xY$uK=7}(7* z5^6J8YZQ$9F2geB)ByQUH&fcmIZ{p7HWyr`M74OK&~lrLczE5dw8R=5qr;Hqz(48< zTsD~{EXx$%B*}teNhaG~P#J?sos+fMy>_5-_Mq=9D?War(*72P-if!jaw1EuO1FHfel0xR!(KTILI~kC@ zm>63-$6{YI{^9VghoPP>tL!&3Pbvo<$8 zzG^61_CUG*U*$7Zgpt#JnBB7JyDgrx_dU&nhBd!;ar6Y0e^q$b^L|%&t(KU7EVPyz zPPUsksDRGLmcy+0nd^+t&I`#SY$aj(<&_5bNB5 z`Ax^>=R{|sQ=FV^S_F>PgMO4L2h0d4NZ78MOi9cD2}ZwSyS}%*eXXyR>g%(~BdtXp zfZCRHgr;M4%h!L>^(?xiGidJsFf-GoT{7@$N>s0&dFuokM5z-vS!JLj=WtQR*S#3N ze5Ye1=#nzk=+&0mwd${mIOY!^V&O{W6b`NVJ_V`3t)wphrzC3gze%DN{&yrwDjU+2 zjCWeNUmv1oZ`6q>E{GQF|2S{?SMHSG=Bailv6S7AscaCil3(9Mb3!dInueryrTu#Z zl%2j(-4(gzv;1~J-2J^sp6K2Nv(3cd$y=o~nSodJS+2OZ{aH6#S&yo5L=)~hI7I4N z+F|xx(J)8E%sZjW=Yer_`|Q`>&r6GK9&Dzsm}^+;&|9L~4c@12FFv0DNu9QUOa9oF zTnU7DFBb-wIm$Vf)DjitsSSBgGicaPw{KJm#0iP+th|reF6q@(AYs46@1C;!Oj(nn z-kJY?r&8d8d0ssH)X%FWvKn(ffjZ2nBZ25O_KV4AsLe{$*AfA6&NGIm_DMtEza8?p zG(KahXIQ;I=Q$Q(Av@outAoRwuQNlo zy7Ce&=UpkIaQX`4X1wulCapJ@bbSuq9ZQZXTAb(4V=q~l(Lq&9p$yT3;pGmdsD5b) zoyKKiI!mbcp*Z&bWGheVSQ8Tyi%xA z0^}|wN)T20VC^BFGBDSNTFZPhzT8UCyuzA+iuTVuEx5xE664x4O63p{d{%3|wgmKO zdpiaG2g+I-dG%m4=*iwM3ftZvPAbn`_eBu_El)c$?L?)4Wft+Ivfo`Q_vCQBRw`iWJW#m1ciPGSJ&*inLH?FP)s`jmKm8>`ptU@_q+> zZmSvq=16mg#BnB}HpU6b$ILa;$WmAQ;g5Qh)Gd?)tM(|8ewrL^R#`Ss2;ph7DuU*} z*d947SeJ}=>-SPNf}C%8Sn*rUE4}>|R~7ub-rfeLu85Op5oC5u-q_t;*z#^+%KAg``ryGO zn@4-Lu0~RKP?>rqJTL~Upm`~7_&`IS>ZrX5-*e?j=_Lyp`+evg)slPPlqkN{8-1Z-3Fp9^ZzAKqNV?@0;R(LFM$%h53$91HA8o(rcQ1sd+TD@FmjBOW&wq8t~g zrnN^27N8QAyv0qnyF#*Heobh(;x(8FIfTZ|xA30L7|Ns=aQRQ3pTucu*O-p9l@^^S zz#;~<&aQBf=?MZMCv2DwuxtS?riA8L%JBW$@4{tEBja*K@s?O_khrIfj*}_v2`)@D zyX(11MSoP=3F2l7+;#GEHB(8%tw%6*Qg_ryG~;1h3$X$7pjX?2OQa7`B!qD%Lv0u= zM3JmDCQ~cRpFA#f1e%@W$qc;b5-e?dkLM)W`#e`&n_Ifse!&V*#g`D_-eeALJ5hWs zQV7`)f-R9-hC0mQo!aEcX6Q+IUOG5NxF}7{F@Jk=KJwn{@nD=qG10>d;fC$8P#I|A ze?&@N7FH4~lJO@$kJyV_AJl4d@2$dh{{RcVj9U*{z_90f`Htu6@>LJT8G4ZmBmc&d z+R~6N25z?nJWz>c0gg8|ue2?mdFKK0bbL@Bf_d9~0%J3O7+?g(lN6||qENo}|54I2 z%be4~&83WsfJq2>rjP9a?|B=yd;Y1)Vw>A5C)?4g6ZnuUsK!Xe=|=eG*=1No<=qab zT#x!-!1m)mdjV>Z|6V%%K{fasFwObFRnDN~%F1@>zQ`MUHaJ)J{B-B>%Z$tKk2wUd ziN{~P@bO>G?ZA?O`G_X6-nt|OYL;VfM!vnIAQieWFmhy||2R;tFcc-{`QYB;%GD&L zsL&%RtJO$e_pxe2g}IwYwd8W+Rs80+aM=n4Z$t!Knl#Z&TMu4f7~3dX>D$^618YzF z>RH10PkLA6fPDnj0sN*qtx}^KzbbJxT|6-M0h@X^m^zPb{8I#5(}5wapP;_C2EA;5 zEO0+?f3u~Fts(5e3iZ@kD^S70KuHFoXQ~upp+Ye8#vQxkDX4?5hJxdR({ZE1{Jit~ zN*O{9jjUGbmXI4n@|)MkzhIX+nQoLD4saY%)I;#CKW^$95nY)PfL+PS>}iaab=sUZ z8z#;GFqJDe^E^%I?-%xu4Wh#iJMV)sZ=l*NGs40q&pGjr0snW>C-TyMo~Dj(v(gIu zF!>RY*%mx}tn!kKnKSPKHHAIdK_l>}=y0?8UE`uaOrlDoaVgZLQK31eDevM3#HFh& zrYrMJn2PO4zC?}dfA}`$d2c+168h1id~o?`;xiuc0b2aVHJ%OLQurZ0b61hKQw>Q^ z_<>=;-@3|5!E#CMd%>hhoL5~+!|V%sb16i%n_{J8Gj;#s;r@D2(JNE>`9*p+y8)B= zv7R2G^P^TS)42q_V#iX2werGUhwY*bD5sNTX{$A(2JoWQ8^IG=mGdp+ces4WJCUjj z8*}RFs`JFciAT%tnP9cVELSoJ_@eO-_>#w?>cD2CR)G3c2ZOg-I5k;sQRj()?cn*x z9qnqaEtTvAPo@Sw=FWx6h_1p_tnfke5xS1c5qTjZ;k60Fe+5l6X3`Xvlwmnh9MKXJ zQUw3Wi-y<<+VPJ1lhu4J9@lrN3xiKS2eAL0MA2_pzZ<*;rKNWiCUQ9I)d3uV#lH$Q zD91m{5?5(+_KM9BBwBbv-|(&X9*N5ZIR>09E_v}B()#8pmfk~SLeFm^cl{(jKrX70 zU*#4%@!*83EWd-7=;3qeBbXcurNj1K2hZ8?iKP?*#2rpqLO1vkTL}bcLIhLJ)~_jw zclH))<0mzS8!n@0V8Y=~DK~VRl`n>CbxE0XU3Q129q?!4ys6Z~Hm~c~Q`Y#s4j0#( zjw${aQ@R5~)rlvb2gw$OmlIo87Um<*QS{W0V0-~^gSL1ft%ue5>j#RZ7n=W4Yfnar zFRsW14O_dGtRZBC(q)8+&WCs}&cfae9TLsILYiwe!d~gAUtaNVhJ+gg!V=C0?00J5 zrfE0us6r@mUbvj@F|vvW4@ig+*CqdMDmYsU{r?ib?Q(vg#az z+vl3kTo!xoU^))jBKDuuD(ROro-+p5Y&s5dY7YE)<;9X9^xwaIgi1l73qk@%IY$rG9cE)Zf*x+H#2B@nx)$cP=I$|*0(zH<>& zoOI`^H=C+%{oEN%E*z}$*wuPFsS(ZWQ@qzjn|H!Qu&`CTuEUC`9O1nGOo`ud9d?vE zWq|FWPsipQQFCHhu|K-m1AkW-_v!KqI#6Ss2~u!uG3 zN~fRyZSRQdin*R@o_AiY2mzB9CCcn=90SoHfBb)$gMHjmGRA+R9m9;;1j6B78Vj2- z^ohmAj!bU$i@?207WKz2VEl$o)S1I?movJt{}QF#fq9EVQlqU~=Rwer#aiX%Qr?is zNmtSK1RIthj0ZmJGX$FHQpwN`phr~v$pq_H(;QIuX2V=;J6^uJ=v9v%90#);)p1&G z({Jq2b}}5KbjM2HZI9{FTy6{bXkNdCWses2Ms#J8Im~_9c$#LcruONXAIJ$E(|%XJ zD8tF5JH5vyzy{Ih_@?M1H}kJ)^TxO5M-*NjZQ`QTCfbhdH&nkOAnlx?L|QTTJ7XkCDQ}nBga|a1tg#4jHb9h`2`vW>f(9ieA4FO z->VM+xNox}2qTh3(}iW4Rum$@g=OB>8T8G*EY1lCLeiA zWg5>3vMm31N!UB}M{VtkZTDmy%Rb*`XHqIInO>X;2b>2YS6FeDx5gfz#O`)mdXa6~wgSeBrJ+r`s$WGJeSEmxmbMOf#FD&p@cmCBM2kLsWth+0R zvxFl*@(ev8_$9&v#qxTUc6jkpXXz$XA#4q>In(ret z;Z?4JeutzwKXI%ZjQ7_61Z_pyF1GX#HcU1k$>fW?iu3C>%!{U#$+jLpE0LHyt`TYy zs*NjuS*xjD%`f^_>;|ob!@p?kHy6Gy*+@lA>%az{gimYpA}YuEK_y8AMn(*uo;VYu z0T}9%c6hv4ZJn~NM}6`$KQl$vdY8Q;Hy0dyzz~hfrD~xDnD1HxFm3{Ki+MAI*tP$! zLAKSx|8vMTfE~k`6jq=-Af&N39`MIVwTOr}hal=Jt>b4uK_K>(E%i^`@@!uhIhEL-n5K;#d36#tg3IOZn zjX>7&HGk10V>Lc&a57v~7U3Eh*CpnZvrbToH-SI-vG*B+%KrnxmWke#DU*f<(*TDL zo^l2$6KVfva2%0yG)T{0u2ND%jfiP<$6~|OFs#{QHEtdw?dWa+P#L9 zupQXcu1Kze;BUd1D=umv|y8z$M^!hWg`zoHiwmk4kLcnR9)r^JvlZUkKdot-ErNj_JQJt&I!$ce+*}mT3qm0qbT|Y$%!F8OD+_cfn=DT3lt>D2idrnW?@YQZ|A5^Sa@w;;n z8|5s`prNLxTj#$9?zFRKP{VEdf827?wWsT6hL!X^nhl~@E)Q~w{Y)1PO=`lAOn`35mFh&Ay0uiH}*s@v<-F^`kJlFaW*hg)no-wUV8X<+G zr4r^0>lk}7P@$X{sd6Y)^oDTmQC`K`j>4GS6=Pj zQiC#Ek8ZUJ__Tm~Ht}G=6Ab>OHx{}JLfu6N`fHw-qBRz=j9xyMfxl-!{1=zS)OoWq zFF)J*@+6R#aC)|2x~ZcjZQ$-#H(y(miQMhnf4F{cFlY{(?v z8;rY9-rj;ehRhGSt=}+UO0C}4f+Kn*2ql7niP0ai3V7ZBWPZ5NT!LYVVbjeGxI5^c z#LWkuyF$c~uwbkp6)GKQt2l@<{bjTxgVYQK4$7n8jN6Vqjhw7z- zQ$aXn1XS}>i0X{V^D8x8B6E)sy7wU*eBWMVN9n*g=nL(8vF1}1e9Y&GQwup1p^d1r zyl3r!d+rVh$1U_<+u^BuOVz1Mo|CjLQ{b6K{-_cg%Hjq2xDa1{r#cen*ETYJYjsy! zMq9Xy2@~2Aa(-2uL|*b6D#?vM(41za__W{8*RViuca1i7h=PVqP7n8~w&8_DY=ktTWg;`XQhEf1zE2PW^YZYdQC8ie#fQvyWa7Yov8> z201u8g1)aE9r|?sG%UF~XfuC#FdO(fSNL>E=zLn;dL3#z|HCPlU1MK$!%G_i8`|9$ z)X5ZO<36g92iELpgjxChwHgoD{d6guBAro;Iv8kZJroSvMXbb2PN{`cp1<8amNT4t zd!0Fd<{682noOV~wEC9_fQaaQi*fygX+Tszcm4OH|99C?&>LSLMO{T@C{G*?U!>(} z6XwIF(HfTe6&sMa{kxC)g>W5sk~z&E$(gXzUcVv61%BWC(BkxMA$)2E$X`1(+{|gs zp7D09knpM5@Wu!g;7w#*LWEBt*RuV(=9hmF!sTp=i~4IYewdCr4<0B?sJC*w-Mr#% zoE#`&=9)k6v9^e62;AoBCNoYxNKh^6n@U=hB{lbei+WvO5E9AIi=n^<*SE|ub}z#C zY8}Vq4ldTo!#YCk>mY2&tj4*zma1dk_}Vdb6^;swgh=SCNl~+G_t(;kj_XcwELyT& zX|EfH?>XDJxOrzjRHd5ro`TGbjcR7hnBEl;d3t;Dm1@uWqmT^D1BdZh^_14ileDW% z%_YFUpZ1?)Qz1r|0fCLQ%T;0NBd-m|w(Y#!JBEP93r=Q}j%!6_VjD1EAur3S9>ga* z#gV~+x;G>k(PWQ5W8Ic1aua8T5Ye@a4>wu$8mEQrYAEqAgRP?n>V9-*HgR|y0QMGn zLsphKV!6_7v!I-U<@*nj>8C8muxC$UQzP>HYP{()jj75tEykUBe*J6oY(1C4zO{t3Vl8bL$#VlJ)N{?JF@I09mW}2>|tNg?df%G z>9{Bkc1_&jxY1tN+Be-l=pabjvVNxLf`Z=4=l`q3{v^IloPo*Ai7dNT<>S_3qVe7h z9Lc%ajLuKed22HLHKGgYm*y8b#4%DaNqtJON}1U*Zoo8~NEk@WQDPK1Vu>h6)|D*= ztFxWgY%<*d&BxG~`j%Uhq4dy@Jay+oNc0I*I)@)A>)+IJTHpCX}a7k=jva19OIvFZa#5t&=r_$cYbef=3Q82Kk(DYMJ;jvt&~Fu z=89n)Y0nuBOct()n=ARA-VHfKSIVK{`*5xj)=pj3glq)8La<28`K9#YlAs!chJmBT z&w3*py_nLquk+2@Y7^G>3|<8{5Vyb)GPgaR0-#o}*4mK>y~Et24Mbp`x=sEiCvkaK zgSOogA<1smx_Pa4Oiy_b1-^4fdi$#6_03=xfqKhH1s~ANve)!C=1h_*JSRs4=EWMe zT>K7zYKZJIyz9*j6!26FuroqD@l;EtM^wIdPACt&B>zzmQefsmhSYZDTlK)5Vt5Cb zEN<(GsxzYd0n_t~s59+c5P#17*XjbhRkc2hl++JbP!!`Ak%wEi=5aPZ6zG85eAr8k zr;lzNoeVOnS?m`I^0_Pks&)5c$y)gmOP-B}ss>wXElo|cr@tZQirN&LPm0!M@vrfU z3y@CAsf{z-vFaa4vp)ar-z9cTR5X9D`2^4cs5YMIjyIv)n<#msCm zrzsR^{-`j^30msV{#8XoRwmw&3u3yt0TpSP^7lu1{h?A<;l}y0V_%yv4h$kfu6L<& zNCf>|;1+g?k;APi)YTlRpCA`8KdQ+D1%pePsM%Z(L=Gyw6>O3ct{W*0G4?ZI=BrC6 z^>sE(p3!n*p$*_yO9~a<OaU1h5H!|~iwF>3|>1!*B7<;kiuWj1317x{>>+{sx zV)T`cJ`o95R2SO~SzTuVM- zqmp;3atp&Ej4oH{2MXBuGRwQ*hxP3b)<$9HQ3MM;(DGnC2x@znWqLr-!|e!Erk+ks%RYv6vvV-b0j5GF&_lgJJ&F9$jyg7 zJsK~*;fwh-Lj0<9PV!jm{d?rmi0Ad*)W!A04~p87-&W1-M8Bs8@K$r~H{mG8fdW(# zx5kbs94Ay8Zbs*Uwev2` zie_pia3Hffm#$@|JDep&mg?G;Aq85S67*}qQkzeLv%cbd9)Pjw zTO}K^)Rooa71sp0wgP~Is1I_lk0v4pt+!w48fZ-qS@sG1B1yyg`9#(3Y?Olcpm<-h z5t=QI%Qm%ZbYlNu%P?kpw)ioyj|T4i1W7o3cosIX-&-0}X~t!ryEGD6!R-J)^L>V7 zwVt20IX4TuLLxLaSF5UYYitpwxI;<6&W62kq8XpJ;w}wH`xM@VTMixujQKa`rV~wm z1;)eFyv#}9QAXpXgN`e=Hak_`0Et;NY-JrvbnkcQsde+Iczpe8xqr>JpiJ+W6pL2) zmetn>ABM{|PAn#xMB^2fcPE$M)@+OO=c_5B@9i)ANw=!%DoIiFvU@6(wq!ngqh&Y> zsaZr!xXnuIpR+lM(m2kJwHCc-)41N$bo?I!bcHj1Ec)X>6Wun-f`+qKc7vu-Q z+?fPWn9b7F-6wqbV{^i;u5f?G4nIsAN`>-QffN`w7#j<_77MY5t5vfwDhFGU?2vPp zHo}grF?zZlmKE0|5J2Jv)NoY&C>K#!i8g9X1b!A(C#E845Q8-0l~5 z-kQ~qo)FaKnIjU{*^3|h#4#$r-ZwuFF*rF@;7*fnfHP*;@!tUU{OKG?bixOlmLzC- zUuo41+fDDG)P40=JhFyiHuic8fR>7=I8 z$S9*UzZS1G4FkiCsr7ppDQT}q@1fq@VAd*h5zrOb*=emumED{wTR?^?_u`JLMo#Tq zYn%XM_n`ZB<*?n9nt4N^i@th2HJukzJh0}=dRWNyY)|*dMaMNXD1{%C@|58+{@Maf zjnj^i;jBP8yg>IUZ3;lIRub5Zn_~RXaXEiTsfN~8TVNzu&AsSeN-%Cvh&X9O^?-;m z)J{JmI-SD3y{uxXi>C(D4M*`8Qfnoxwm%3vZK|Z-9G!`9%L2{sGbx>32SP8=>Rroe zTjD&u$x_DH=inf~Y1f0AtLWeN9$6RTG2Ex;UxfrNud?O7d-?->cwr_{zE9Tf?`Ty9 zu7gZxMbcVpz8WBe2l2f<$05poDa|y+^`zE|@6DDfhx)Be9Dhw-@*Y`sC*$2S{+x;| zjS%k1$g5yX#+Ejn{e(evja@opWo%GU^EtZcPSPXjQ~YhQR4*Xx>~JQzhybCb7HuxB zR+1oII3xT3yM?Ck#s}goMl)vzj0>?RJxJh=2F2qck46g=d*=ZBSt13nrU6AOp+_^m z7HH)4$E&N~Zg1Yg(E9@knu<2VR0@P~8Fp0c6$uUg+|nPJK2Nzn5EZgK6;Ls2{^>U} zt9fKy>GdQrN-k{seoBrSy}bOmJ7O+@7El(d-0WyQE;kcRh*h$t&GAZI)HUS_aR}1m zTXEz(k0_(rEtjLD?ZlFsv}>1W> zJA`ec_+cViYbYuGVA||Db|E3K@J{IP^M1;eA|ZTgR?5~v{$%Ww`9eGY=VFC$%nFGZ zCX6@yzVSP{5TL6k@SnW^i(0qiIPZ+Lq|u1-l|jtP6Z!h9R$kLxPAc*?e~K!V)I9j$ zW^E&T?4p(q0;1Hl1j9+g4cVTWoZeK{mu50vXgU7e$de%yR_T?6zIM-tP12ip>np8~ z%d)bKyJaR%$(F4v-e8~5P&V_n18b0dN}X00>7tcSS{CS6Ehi2;CMkQrA?`#mUr^>7 z5xgSmndzjtKP7-7-$P<>NC?y&E6cqL)LX97yNl|+?wh|J^+k4O7(Ov|{G+z>$NKhz zPbH{~13playR9DFR^}TZF@8G}j3e-e2Keurk2f9i)EvPyKX&=3;!JDG7Odb6X6Y;1 zj$B1Uw%8(5nt3>92|Q^ZXP-$kO+K;oglqP^KK#-U47cQ7*FPW}udsr@#ii*NC9}#K zNCAWq9R40S`3YaQdCVBO#it4m;;teatMMJs?@BG+U5@=RtOQ2B>DRw!5ND{{<+DgK zONJHtUS?dyQa7vOv~<9tooY55lCg|iguAW9t?m|gB}_}+XwGkD|@e1 zHjxjP4$Wts0%nV|BSq-H7%)4NiW}E-lhzHTb5F-KpinL+>(U1McN#SgATRuB+}4SF zi*rk1x0l{!cFI=L_S}}|L3lg~LQ8}iw!(Ht;6?r-XkOm+N0#U}$?HsP=AlAgH6%0+guo5*P~vNsK4t|MVY$zr)JuNZbj_!2 zDpB>G576+@Q~(n*;sBHKnC2-q$0$% zid)aju5{l{K0c<-zUgqisop$EMO+5%e_V!Q0oBzzUA2(Rf~7~)Ek0;XBoJ9I?Q`*Y zZp5XS%5&KLQA?uHUHuDIf4R~0{kvBC#MXsV>ANq#%+C9H%z<}*s$46@;SY1Z64U`LAD6c)%PkCe=_Dsf?y}mrToCJ`_|*# zU^m<(x4t5suf*=tA_TNGVQYVUi-Z*7)OYy);CfBm&RYEfAYPpBv%Un!IgRg5m1y`R zD7p@_|KwqbgJ~u>R6zG>Fc2!~Dk_P|~{%US_KUm^}}sX)sD(w}L!!4(kXDUbZOP zPW4n_@Hn%TznC%YYB(htNs=F^uwmuQQc2T^crGACbGL==wjsriycXfGtiz%+k2j+^ zjkR?mMS{b?g6{oZXxT*7;7T!lMpH4^Jxm3u z=Da>-O#9m;IjcW$h(@LHLpMiBX~1;c1apTju!Q=;Qr_mYTi$vxa6E>2W3f`>D(kFE zor7TM{v|+w;Nxxd|KRQ|!>VfAb$=BV6#*BnfN_m+-}m*q&hwN{tR$IKR3bWLcG zJSbla+jjgq<0V6Cx#c?fEHG?eQq8BcZls=uyA!ryRNmk0f(imAY01(^gtLjRC67OL zH-B%Rs9Ap{XiZ5*vfZ&CPu5-B5ijz`Ls}H6JM6ELy1b?)J4IE$oiW>N$)L3iVuI?6 zUh;854@4uY6L%8{q`ii#QET;&SHc>g46@0bKNpikj$2H;bT6JX?@G0$LW!$}bi%y7 z9|>r1kPi)hnCKSy$fVsj30~nToz*oTA)9km;+9SJ|1`Mxp|c#fn(-zKr1`@VtVL~+ z9k9?-wdTF?;#z!0d+LxREdi@L;7z!K3t;F<#@M_e7>K?nwnO=Fvu-PgoRGq730^#- z;A9wKXYkDAY-Yr`hx=-AEG>`Jk-Dq$; zWfdEt24@hDE=gs_4Ccq{B?COpRoa z!ETNH;S-W~vFjG0#v$*$y4M?RiU9W@5=&<@dA}xY1;BeMLbn$kfd8g!f9!ofkRD=iopoQXzZbl`hk4yvdM(g!GcTg`CMP=f zRonW#S5Hw_OxDpFE!WG?OaZ}??Le@XY7xrseLY3cZ98AV&Cf70a z6-OoP^-)ZRB^zKxCr4tPf=qc^lw9vrki`jn5Oq#`8OHAm&@e5tN! z$&zuGz>B&*yb&}~DEc-|$#!=pR(!Te+cd>HM)p3b|5#`@Iyo$)QYtwdH$S&i?j$mf z`N0E}pp6^9nr42>dQ@eL%F3e>LJ?I=ct%ahw388Cm|+0EVd2Mgm9?~)Z-P)Nh4BMA zh58^zpzW_}vt>Bf(qNM9f2ndTMs?ZGpu<53r=`IFq+1XCXUH)QT9 zU)sz`ULAKCI50Jm2*j_7ct>enK(S@cH>|Qnw(%G4O>op<##h>df_Our5 z2+ZaF8^hKOWFM?qa!_Lm9GL>gqN6&^d@1tug^Cr&D9oCvbtLP;EV3=zhQX(-M0JSO zlH+6{loDRb5|2jE@y$7%?1H3Htx46B2Va0KZ7)2-xbTOTqQToX8 zDV?37lB#~~UMeQ>=-hx2+jh0UhEwW94BeIZkKhqbw<{74#82Vvqz{`ME3VgVMT*Kg zq3?MGP6;E?gS6AVT6_GqhRTrJbO|LkIKLLpMCD?O^nH}^_+iVHx}HCMP|n4IqjdWz zsk+RDx284{e}Odsk4Yz#!wl8@?u;UTU~Ofx0Pic+Ui5Gt*w7qg2e)qT1@dxY$t z?|boOlei^G@~|b`8nD7fY-vuPWCZ98gp;{t-Pa~C3Gs-!MPM~wk(vY>TcUd@FU!1* z7rY^iM1@uu+-Wrl%E28T}vOAg}82ZTokMpNf`k1gaSO# z$ku(Id#Omuc5_JjOPa!G>tkm6e6;#QBbGleK(%bO31qH!F^XmM9QH;-ZVF=C*L~7< zvOUcP$d+aY)$Ch8L???g>#xf^sz_g}WKiFy^XS0^w-f%OcBgQzfO6h9)eYDBU+o1I2m_ zRb)gOTZB6JB8{1Yha-M>#jeLl2$G?#gT0o;FVa9vdq29ZJNS+)B|7ez{ZK_F4Ad(Gw8b$XN&N+XmxnEX|G} zo@iR`KzSKYwk=A5mCzm+FIi~q6OX;hX{Gp7Gg}2 z1ufyY%jW3v>i|L|d#Bb@4L3hd5bn$L#sb}J`>}1d<6Bazjv<*(*-yWhhl&iB6UtT! zB=HmOl}V;b<6V5Rev~b=HjdZ)U5i~)Pj{G7!^Ao;%<4nmwfs>B?{yJ1GREP~4Qce# z)3*+1Nm-Hg_IM&4O+FrPYAoKGY4XTJv6hQDn$lXF18tYR2al(ZKNkP|svUe0(A-aS zj#%HAZ>AOaNK5Xj7R!IC+s#zRMpe7R)j4VyR&d)%66){m6I;)+#&#%SS5w+S&_jI%A`dGbDTDO zdQXsJH4SFwS)|xD9wEI%tg~E3?OE)ie*<7M<<_!(Z@m?}oZQ>13 z2;;jJx?W6m;)`oCcxW%mP53TDqdWb3VyCgN91{VSvEdF1tH3t&OK39+X6WZWx zgx4ZB9`AS|XKSO%+V=G0(8z)62-Zpd z%kDP2I*zzry^kI;v~@*x^p-P2BQdRU37CVW)Y-=6*7J;ZI!$rJd#c|Lsj3>FTntJz z<@)a9aeT*S=^BH_lTBax{APycUM9vu2nvlQ2F6YBW*&sUgu?Vby%f;kQkJl!iC-m6 z>lF#$q|-*ib8UmZP>2Ub|>pBQM}7Z>fx`%Evf)UPgOf7YiES!7e?vxm9pwkVgRSGT+OciRjMV z%p8Rka%ThmmzZ1S?2SSuDG zb*o>5=daGR8fB^f-k18I96|zXTd8gJHmg3bM_EjnU=O-1Nm>c4=FpKosF?4cw39q` z%F@S7=hbi=>e(;+ke7FwdPbkEvAc0ycW>nF!Kopk8raAg_c_10z<}XF|LSfm?Nn## zIBU;r_@;eg=Tthdm4UDXChkPq{)?cf-bx`UaD8Vt_=kAC-e1bhBgCL<@4^&_| zHJSfJ3eCmrZc}XDT&f7 zZHOMImhWlkjADm5Cl}lGrW?Om!wu(jBhmR-xZlZY>V3ofkYyQhkw~acYV=Kp^PCC@MkO8;XkL9X3 zGTdT2rJ|iSY`szBvRWzVvFBF8;B~N+X&H8r;}QHm%%H}&Y4Y{qV-uT}Bf~z3>Ed*H zJ5$q_Gixdfy8f}}?d{M zUNYbjE>^^?XiikndN3<(EcSM_B)ht+zqy*uZqHAsEji%uFS&%wMN-jo+a)}f)_uo& z5nZt)k*l)yn12XJZK?aU`T7mL=hhg*OQo>AY&I6IeEaiGwL4xZeUKoM{BapQu!eo` z2)1+#`0oW^KQ>^*E$tlc<6P%MeZ3mCpBnrcltu0J#vwR#@LaXWJgKdtG}+; z-GDwQ;=4&n=K)cX&Gpiq1nX+Z^4&8j3;tCOKm&E9Ei|LflqiM&brUIY5 z>1Q`i#@@R}hjEukRZSlXLf$#=sEM)^SA}$D{GipTtYeEy9iy(f8Y^%&qY5ipvLcGF zbXRk(K3aDSeWAKRIV&b*iEsek43K41<*Qa|{1$ENl{j;mcCX3^AtkVF(HG%&R1qy$ zsk6)SFAVR}XwF`Frw-NDsp4!y=EbVfG(O?LwDO`(`$?tl1Y|qlYYSeO37YnxgS=IX z`xK#6dNpg4=Ifx*Jcg7Rhmms?;Xh{o73g9-nPw3@4JL?7NJl8ubi4z@LctA@)0XUc zXZJ_T3vg?=;_H`<^nOH4AAhW0l*tUPua+yzh*+;gJT%=Mw0dt4 zQDJ-Xy!c6QJ4y9W_C2!|PeWSsz-lIkiFiRXvd}ZxL9(ME)2jMw+YO?^T{*D0!6=K@ zq6=Z{6OL0Q&8b&BF~}q-ot_ic(~cuDDK>FJkz_})h8v}S}17%%z+t_8m|CzVB2!;DG|=B!Si;ndNG_?@9t#0rXA4) zYLPPWMQ<%Hpy3#S+R@Zi)0=`2T5Y`s0@mR1uOqn1W*%6j({s4Ja^^7vz*#^5T3Ln* zY6HfgfxVF_!MLEK?XJ}rDi&}qFt$`#nos&AriLYMEyKeM&e3%)TD_69hp7BfCn#1vBmN_9rfr__!f(LP?8=G>oYt?K%w&N}`$z_}Wc3cXbm-5SC_zRHM#A@i>Mrvh7H7r>72AY4W6Z*H0`K?R*K%-!wN0Cc8z3 zWh}A{vho(7J#n4>LPetItv?+Z&L;Noxh})6YqaPst}_~|F8QS?bd;&;Ep8s#W-D34 z+1Z0vhntU|%BA&$-?@^*Q=zvK&K@^Q?*cWnW$sWn`7PeuCUIn-U4BWI`}J$1^3qKk zc01*$IMKHbshY)OMNnF$3x5CgKKy2ElJKh#K7e-dJ)OtAJ$f{E^uP+H3JF`viK1cJ17!_ za1j)Ix<*^q-tXympR>@LDgtLQ^ziJ%sgMkfG`v3Q;m>i516MPgvL&kn7FO}|QEf9! zSzV0h8318An9)Y(mpkWpRp&$eH^*( zZ#rD@K&T*AqRq$DIJdQ)my|wK9>{;Or+^Q{UV;UUtTsRH<6m*5)h?01q+KPli|tEg(ceXl_q(Xg((zs8_rer7w3cd>_w{1;?Xj+u*M?$u z0s(PVbTsD%UE?<#b6-tQz3;x{Xjbw$@ z-E=IQ=IO0F{Mf}FtinF37%bIQqD4|Sd*C=b?5)ov6Lm6W*Wl(Z^Y@bexR)Cc6d3Ed z2$-q-EYf_4VFL4fGMUv~T-{yzM9tJmupI$`X(yJQ;_;Rwc1Yg~C?dZ47qZmcZY+jL zrM6*Iz60N`aDbDsIltw{hmo$Fi6>kq-#*L&-$v)PsTV|r}H@ORZr)$R;E)DfeCVi23@BaPRBT32qnIf>nWKFF-q?+ zyD#ZR7!LcE1SDhGUDSBjENG202lo>!uZ7>UZV%-xKG4`8+32g|ulg0|fX5MrNF2W5 zg1HdA3eG&F?{1qBzf!(<)GeZLTXog{%$n%i#9Lni5@NZoQj+q!Pf*VS+i=iHr>ShQ zkeMYm4%K4M_>5F%hE1-|=%QyAm1UZ7PZ*BN&=o$4mn>q4ih7JWPt3vFj4~QoZR%oK z+#)9U&zvRxu2L5o6{1;1NOjFl=v`s#1?Ogfd3Bsh{6LMC?Cr`@CU4uFXHU#!h2r$1_eZj6=&v%Ea1LmLW znP$y{d|G@|&&E0h&o~q{wW9e?8$uf=XqcUM2Uly5huhsEvYQ!~{m`9%thsd48mJ2c zsFS1ojv6{0kfet3H`Q3lnTLQbx*6gaoA?Bd-ti9ff8RJT9Su4>>SrdlxTJYXvF-&gs=9ifX z>{ae1#+!wdUoed)LX&ki`!{!H80a_dueBcFmxpqgG_*#n<3~M@gyZM=I`rsHXmSFc{W@ z&|qjuM!G++WVjTQK!FAa+g_hF2EtNAY(;| z7D}od7K`>N-5MA2M1a|DrVPU-i!Hxj$@3q*==^Pz)~r*@2g z?BkbgDSvW`KyBiR#yMNC${=$gma!_0S?a}dMm0&TK}W4uuKX^ZK6ZXpLz=F3x2%HG zCf5fms_rW&k*^=YB3E)r)>&4XoOC>YX(!@Nd>O~u7$HZOrcDiM!Y8Ro)U@ySM5-$a zzh3DrXgZ7)6a~v)w}@=IJ7Qc1Xa1+xK1GD-DPGG=&49!OgUm zH#mRX7vi-bG!wTYa!|C`5>C={hiffWZFsd7*pqLPt~%*|L_PNj!{H|Xq@FlYie&$s z1^C}hK)XiIX`rQ)wQY^I8k+XSv>r^}TIYoaQ1x#`ETa||1~SCU@M`taiZA9G-RD1n z|8iK$&jc2M=hit+{?!@ z&upn3bZwymv4KuxJqL1hJIq<9Y6@Hi^HfeZXOngzz2EFNcyc+CV+^K0n|;jZ_gnm( zM|qkq^edLEpZc3nr>k|Ng`t6(0Z1vOm&{eIxPkw)`@$t!!E;H(Q^Vl$6HqGA|0oqT z;-W*hl8G|ht5r3Gj|PVvk;{!>Zs3}|V-bW$g@S{{+e|+>pdewQ7CoCvyQj{u^iXsw z7-GT>yM3MJ!tQ%Pk>9mb(2M6>g0em2afnHr`le-C*V|yU0h%Ic zAHlX|c&=yVh=Shei~~7CS!=+^Zz2)VlF;ALw11Y)6&;0srhncW^<`qYm-LU48i$v* zs|MJs5*b0U<@FtJ%2@J8H6Akmon_t;*mSJI@m$R{lQ=DXnmK}2{f2Lv4pF?OV*~lj z1xzvW~k*E0qWL=Nf_ z8RNOAa!P<6hB0@Q1)6=6piwB4<{P37&T7#GssOS6j+po=>mKjyAbHYgYYY;?z> zlJ`kIMq2HeF3|gDJiSvp8R4n=<#<lw|icJrw208KTmSnQ(G(omEoKO-@#wG!24-WznxV06sVAOdvH-CZv0 zgKEhosOU%o`|odKKv6WJE4?5qTx=0{53O-j)!PzfoZnbQ1rzw7Au5R0av>2yED|3x z&^rL&m6C@yj4NqOI;+)2)#za)|8r22rUjon?qRN=BNv>O@GlwhZ^RY3&KtCfmD)M7BLt%_+h*d2L1#hGIPtFtgydA7)I zupnme=@>QOLKDkPXQ7n3v1V5|rW3%K{O@s4WK?dh!lpO?$Qd>B_bF3%g(*6>WAO;rB#zhRAuqWI- z(%tW2IIrp1W9n|BUC+c>$^HSR4IlR1M6AjXH?a6Iq7J9TsnZ^_M1izKbP)MLU3NC(=A{9MrLcJ&Czb6l15O z*FYCGI${a#5y;;0;LH;%T&wI6Ncs&N8?+wA&VYxElvb)8EF1%M^Oz`?@`4gr$>YZd zn?*(#(2stOk$x2S9MB78*`S0@DsC|?f!3LMb!S7agokq7?PWAsdkXP%^v!x=Cu`nO z9ZT!?B!VCTPB|iUscq5*1ze|iI&MSh9P(a2?~r)s>mA-ogo>NCC_@D2_xHu80!B|g z*6wjfav0;-#HY^2-!xrr;noFQGjrs?l_Ez@rt%vaPnrAEY`lfi@Njt5HMz=}obgq) zC%-{^L8T;r?ou7SI>1mf3Ka{6oEV_t$$JMAfU7+w7WRvQ^OlQZ^lMTS|XU1FSb3*MZb4TWOt!D z9pz_B+gAq-&NMpr$K`@7+Li?7XoYb5pE2Trp@XUp)g<`Z@V&NHZs;iwwnx-w_fJYx z-v}T`&u!@fjePMre8RK|Ln78plJeM|T4~kv#)B6Fg}n#}OFf`S6n5YWn?H^@us=!xy#___~9? z_}aLSnvE|lT;Hku`dRYvrgd>W!rlGkWM6lj_}#SR?|xb%4vDfINN*y3;?(VG9=kQl zwY1_QnQilZpB!DPm=oj16gUp-23Un=9Vd+CQ$etaNi!eFSh@{YdZM}G98ih_8I-%D zz6oa@*AP!ABo%&Sek~sux!vy^wIhLIn{AacHAs?tKGQi_{!|Om!3{$^({GE!GjnXq zz=f%DrJU9i{9Z=l*8BODO)Ez@e5ABNJd8q$qEnU((42`-xhY9IvF}heA^( zx2M;fL^+>l#cdFCNj!*7XTeFoEA5qc05fLx_to`&Pchc7Jgx%>DVLTmaL$}xZ9C0AC z9&z`&>hwDNWqk8zgBN>!km!Tuz_cY`mMJPeDaczpJO<_{B|FHFm!C?>Bc2ViL1G`F zYYYz=xn>b{c-t_*Q!PqCD^O6#9eBBWzxb}YnI+1!w`6^29kQ!d6J^OK_ru(KDxYKT z{YMPCARKG5Twi3Lbp(+q#;5cQJo^imz!mfx=2QzA5!8WL`@^-4d{$2dymhD7RS0QH zJ7>kCZT%zN22VSCzNOwvm?E_Lm3Mv1T`b!yMx$Rj*q-A0gD_~YR|j-~48{LEUY@1LY8sfDtgO=;?MX^2 zO=WPvtFjN-cY1L5_3_x(xnh2tvl^d^fXamOo{yb)DDkLH2ZmKY#n@j~*zq2#I_*|z zCld=D)AYMF_Y&QfkbgEHcWn{Cz5Il{ESMrJ^Tb;{yCVG!#b}qUUOxpD%JxL5>Tr6~ zA693tK+DK6T1n%!V&GI2_TllS%(nK)ii6MB!~5&zb>yym3M?V=+z?(9tP&*6xI!9F zz<;Cw`})#rzj>M@Jsf)^RX0BsKXVP{L!o_?6&75;R)Ld^Y^R0bJ^~t;L8M53oWwigrlS;&6uLoJh$$Y_dOiSC?IRYaUH5r`5X8ZA zc;6gz#uMYoezKC&5S6Bd{3MZ!dEcv@35L=%2=e%EM<=;zhYoqfHNCQ8^XqPHB=tD^cq|2CG!aa|mh?P8b+EUn z^p_b@2Pfd#J7H(kgQo*eX}|{I;$$Iu2T2be`9GZ$A*+$m*(ogi@+tDW^mdkdrKA9o z3F@;t@&}5eUCJqMA)54DF-9J5yU!H zYOPW#CHAT{bi)vf*0X5Js26d`&oP9s76Oih+CVv}96hy)Kv@$07rI%F{NfFd?aPwQ zIc#r5>XUPs!k~*C|IYqK*eB~f&I-pNU)azZYj}<|n!@pcX$s-wWi&Zu>zb;z)X+$D zHOtDRFE&JWm6%&L4~t$C=eXx07Y*aAzpDP;b^o9&fTz-H*`^3BY3G)k)j{jxzcSF^ ze_@~(v1vmZ?AhbCf#zG|A?!mhs2bzkn${+Bx#kI|FrZ7NFGyOG>gPJYMcFWy`%rp{ zGoJ#%Jq@oUlU9=V-8iWtbM^7v2%KL^7D5~j zZG`XK82d~}f(8mX!M(EIntHC~*y9DQliP`I}{~ri^u+* zeDnMd-HCP^+Hv1PFA>d`T&X{kDq=}gUal6pr!RG9fGjRnxD&CxKWONFKukF9N-KH* ziJ7|Gq4virUK$(;rWkq0KLfBVBDsd`ernGR-?0031Rk%Bot?s!jypTQ&(Z=BC>!_uE#CJT`tE4!uxiJ6{7 z9F~Kz0wZ08voX>^VV^&Ld_$0TuScM(Q%dl(edyOeIFa7j#&1ppQcYbe>ai`fA8Wo`~6c@f57d|lua3lNLnMhjJrBvqI#jE6o={W zIUf2|i@)Z@Oe05+jWM zeUG{Z$uC`A@TfePyryw}V~)2UffDW}P?|4&>*6(?_70=79Xzg>dWGWk3g~vyYT5(oVFo})O|6^) zJeIulgiyWuJI8Buo6*tHEqB3yivyf8&)(;D#gaFOD~oLbR$s3{{@%?DUd&ZPAlTCX zHrP;za{%`Vxd+tefKMiGaV)=b)-#$FY*U{qBb7?v#hGg+OHUdh>vLzPSBH8EF{Fn-ALNjD^q3i|6Z!*Wl}c=K;~& zJ9X`zH&027D>+q?QXKY*W&pKv!a#VOrh zv2M+`jFlTj6XNvu7{`@cIqP{e=yDYi0%N0rFA5oc1POi*8kpUcdzrcl9J>Fl$|dF4 z7jNx3TD+TMuNUz{;-dS?4{%;xx_>z0oEWvxcq4hwb=?|yGOals8DFCC8}w7uHa-2s zlTKSDZrZRzUV4>Kmx)nlzPaeK?B#vhKtHl<5X>88JlhUF%l}?!tI!}DyIrI($sl!( z<|Jpo5YD@9mxAv)xCyXODbjS%@~)h&%U(mVL@t8Y)V^sX6jvRtWdmmNSH&c^wQ+yv zd1czZ%bUv{JZoQ^aGrE-Dqvh$&;|~edafS--W~B!ky`S7jB*J zs?!(+UQbNhs`K3TdtdzYe&mem9fOB{73IGCpt+ep`9Ob6EIjkt)>&)JQv5!+ky&)W zp!aBdkL8aBE;Jr{fx2%=!0fsY1eYwv-&DDtq@b+c-t@tulN}72Y68{JVE_j7aE?ls zt!D8a!SBo*>(%!+$qI$`F8V4tq$C__(WZKSE^ghptUBKQb-+%s4A8_D@OHPoAxAeS z*%~!_2hDv9(8E+2NYL>v6_?$wqr~Ey(f;QOIM-?=Pgaz-Ba<6aPwWQO7u>n&rHn3mHoDD4*5LS7!|$1uhYL4pvse3-$T%XilS8GS0FXkO;cuY;f=`?e z-$F2LuIkY*bNWwY5|19CYv>xZly+AXG5Hj}o=;EeUVVto*>v%m!0=CJsb}cNNW0*2 zGm|7seQ?Q6b$79-u3zAD_E|FstOTb&Xj~@1`d~8mgh5s5{=zK_t@ooZt4s^~-b6Cr z)q-sHTcP4VfKr=QEf*)9P%7m z#^ezNwpGA5$)~JVAG7!uktcSAk^TNAv5Ks2_e^8rzcQ#?K=E1GuP<67Ysa9~4MK82 zC2BNj1-F%E&Mvkx4nf#^(U08P0;qe_zaOqYo@A(REejtAu+Nu}>OMCE`{zqGqsT`S zVl=%6Xc~E77H5VJaP^fUbp3TXQunqct8n^jEu?4?DRXDNnXq{vG4~{f7XaNvky6{9>cPOylTL zNs`rl)Oy(uu+e>l3ZUbEm6gW=&49Nby@XLpNF@UKX|r%vCl};C(42fd>ABtFw+PT| zWG%NO@e2hu&|h-MbMiX5SAcXi`M(1?tuX)o>z8a1-H;^DC6)drJ1`ndrQw_T(m5e! z$$ZfcMYFE{#!{P$-Yq|IgW&r*VU0})f1MXzx<%FP`4PDBC;LLqLrum7X=8fBTE{v=J8ErYoTA ztCIfhtKZgWqwP3i<*9BVko@Sl3vdK&*&Y$B@hGqq%S#iPj|clzrfQ9&G#-xh?!F^X z6GOmmI{aMfc*+***r=N`15A6c;5^;fVeoS>fQ0{c>m!dVZwCd1L8H=rdgJWy^f-@RjqFp0 z$q^aR7784Ej<;AhTcsotc4 zd$2sXiTZk74dFk-E=S~gSKE*XF3|-e#;FGjAw0TRP4PlMPFhd;*a_}%qk}HWEhoVA z(@84Fw@eVh*JM=8Ml$lP1g1G8Gc7Q5?310Z^f(96vCExa+f}fyw$}Hl(lw18CnSUp zU8dn~@e6%J$WLt?C*%Cbc;vU28ZAj0M72Ewx5^O=XXSL;F zF8%3aw)e}C%II48|1rR6;$*f&Fia8mWqg?30z>nxHMlfjb}Fwyg}r`!tO-Y8#;C&B zivBWSaSZ?e0O_pz^q(Z1J%s-kNN30Np3XmK0sfz-o@M`&)N`xpKS4e9m2X*=YccJ{ zvj|CWCQoQrS_{lA#`%>$w>SEYG=?pk*Y%-xBxQ^lA(`bJm($I$!DjM!OEG#3KMnl5 z=o1f>Teavn`t)MRdot=cgCetStPf8H#wnx&kLV(3*@pEG-@??YS)8Y%?6wUFtO)I~ zEN;GEqTIig4oaAM)EXx!b0zPiilf zmtW(O{(9?p{w97Rh3?Lih-fD9l5BiAihryU{qVMx+D*WjZ?a~tGYO-@?N9%Om1z|# zhNH2cdS@zpv#JponnV+}G{WxX#DY?aVTR&<@vlTc>C5D zQ$a!aNrRF_M$jYd2EOZgS0VJX7!i6KTI|!GzU;zp-@f|rJnHRJiMOb4Mo_x<2M!|V zFCP@QjRWLo;Jo-7x&-)tTc}NUismPpuKq<(Tq?7tct&&O}@)9tfUa`$X zm0r2-xc~K8ku)zav&zbt0#eq{nWTOH7A39zh5H!q#O(>2;sCaR=|Z5-mksg<_^(-3 zOw$t7KCF%y7T-a?|MKFpokfyo1%G}R+AJ-u%F7r4dr&nbUFS6N{`5fQaN3>b3(izI zJF2iW?~2QV-*Kly_J~n_)`fwz+=Jt@=ivy0Jmw{+@w1e3`t&_IhfA9HGYVU^HtyoK zV})Q)Y@dnnsiy}iTWLS(aU+*{qD7|c{p-20dPP62qqCXqXvX;NXa`;0xkn1TL&;*g^Z|ckaHFk@-+^hn)r&HaWMQ!*arrGw z78ZRpI7oR7BtEAU(RBps85T@gyA(TPjnqgGUcDzQTAeQQ0&UguM@q&l58)5i*l(0W z;oQWY2s^aq3$BR{zC4-f6V|eaZdNam(KYHk-dk%h@QEc6F+1mo12g^~;XiVCuS*{O zn-y~r{@<*ag>NGj;JL5BW^38ZX{x3;V&Uajr6|g{c6%AP2DI15gal@uzJTTXsBsZd zeGf>HBUt*MpF8Jl4OPxyq$COYVi7^?W@XLj1X-$+Lv z$sgUwcuxr2Q3iPpZ%x({2x~l=e$kRyO+-K?q|x4r4%jU}jyA4eh_iyGIwP0bg4Z8U zjY!Bk#^~?0XUZ)^ShiyhtcA4ntKa8tUnu--KSk>Q$+)mq3e2aGix~k-RZXT>IvD+O z)B{uyKG}@FGg2dLO%C*=e8`4_5BH_R(m4IkPPn48-MHqGq~_MgDy%Vj>3ub`|0f58#Tvc|Q^2QACu9#(dYG5%L}2wV0mW_TrGq|{0_9r@8t_a<`St5>%V zy|v4tPAiQ2SqvqshDgKeAz0<5LM{0ODd4Pj3jM7!>`F1zw+?%-iphCbdXWy8kN%H$ zHLsxHW4|x&GgF{(s%(fjW;svxbo6GKRH-lqmD1s*o15_G*d%39=h{FYur;!stJ z@m1>>aQ1xpOuyHe=dX~e)MSCaNCJvo4{mr)2$q^JIZYp6abMb`K} z+tcPPehJCXq8tSUSH8S&Rt~3py(I#rW2C^M|VC^b@ikc+MN3Fh&Y7mhxV{ zGhj3G7R3{YOhCP+zBmEB+=PX+>yq9ClKLC(>yPgzF*GaZJDLf#S+i^n~}Pji1Q)*jRQaMl2Z-@GsX}eUG0;GzgXi%BYX>c-(b~1JxG-~@HbtKigWcWdS z1QBYqBwKMKco)pMleIL|%~Shz9C?S2#K-$V_-zJrX_tui7qX>WQF=9lcNK zL1LE3_>M#!Bsz_u)z|lJ_l-C~y~B}FwkAwDZ_)8NESI+@GX6MKDj397@D0Tt%|Dotf;>iB1#fIyrL2r(S zKU^^{1r$E6zs-o4f11=ipH?KXvSx;dV*$5l4Z}(v8Rb7WG?BdtHe^GTdkuXqfi2;8 zHZiAZE;kuk^e zRK(fzbhmMLiX~iVf!m)`*D|9GG+$1lHo#PebIINElh@H^R1P$dKW>$uvba!gFxm2@ z5O7L96A~j*?U0@L!naUG#RO%E$ucjj|a2K>zF zlWu-gldt|&CNC4!S83%WIL-Lb@E^v{wYHVYlH2rZ-mK^K{%EK3ryfJs94vH&cJPTTWOE{) zqI%_dj&j3giDr4+rpd!_(_-On*WPW1409&$AA+}G&}4ZYm8K(-VhE7Y z`W%n)DSGpdA;)CxCdOeW`m~QbHt%4U(dBDuP(TT4mdRMeXgqslK&}xqnr)4nrv?Y} zv()6OHpi)X^T{d7E_|i3bs>%AF7`WSp;eKt8Qm#DsOf7r>yPjO4#=B0URQ#NnJZSG ze?9kp8bG{OZyiym2I+xsSG|+gWZas3vomZ!uD+A`J1^55y7-CIvo`=(5rxnMR;R(N z4j918CYP0xCjV7XM@P)7_wPq(^S*6ewihpSPTvy@40+|7PRNt(?|H6{J9Gy8!vUG* z9}dWSH_B!Q-$mr&=y+@f` z*R0$46WYI*p2iRn=gF{zoWfpP-w^yx57CKvX;=NK6?5qheuDW@*tDwaqgYt2n(5r(XTVaI$I&+wI3%6JWNxFE%ZUY=*>ZHA?uCsN1s9XZ2l^!YJFa2h!|{ z&AdIA-ttOkwyrfq=T+E#iFeCie7)WLCKso~tCQ^mGi^5)qvTRa4K*tcAU*VdUu~AN zKL3B@on=s5UAv~EK!893!7T&}?rtF@xCCij0v+7lA%OtFT|+~FppCmju*MsAx5nKY znVt9hzL_^?s%B2jnKM&U^N$}u)mnS)wby#?=f1BYE<`&>*MTK=RU-*B!Udc7aMsD8 z>%x-#$W-8m*~ZnSOFwgC;9#+lfReU1)cz>3_8g5?KA?u0q%c6y@S28y$IY2e6OT6w z5##v^cjV&vtPn$p_~n<1RX*+Nv{q}`SCsQTpGoL!y{>7!1$DpTPx^nj)AD?ttoLqs zhzo$c+V}P+?ZD07%8oqoo_5Fi?F)^Ss@fri_a!;h&fIb%qx&m%93DQ$@5+t+i}C8P zC9LDl5%+v;~S zIMXGfQ8+R3y2yjK8z6e3pMZ5KO&V`qa@Gq7qH3q1kOE!dc-a0eZh0l>+VLfs#fUZ6 zMDDyFUxbsv%4qdAzdf>1E)An&X2H-T80wORRD+lo*4tlxWf%tq8mhlWcT^){rD z?Lr5TPapOSF8=lL!U;U32;4i~0X1*20JH&FBe@LafC1{W*hkUmzMG;}=*`LCE^gkO zLZ1iUtEuC&==m$YFxycP@!napk$vhIm^GJ2H<$WN2QZ>{`JLar^&0`}V$V-X6nI*y zo#<`DLUImykG&_6E1~H{GImGkrDF{S842qBK13<40E@V!5Lm+mNRFM~YdCC+Gj90< zaD1YdH(TGfP66V9*^65@yvKxDx0Vf!HU|>@If?MF{2t=Co)uD^@@fuyCnrL35?yCL zuwaMN<{t%TRXwWm@{ZiTb7VbF<0@yzuTw~p@WCp%Mz$Z%e63$mo(uP3D{K=#;H;+X zQfwogO?K8jdfhE7u64!l$na%xXK!HDp0ZO@x8s0QjO76Q@bD zbFWL~#iYvA^|E2I5LZx?Vn3qlrKsim%heo;l@G)I#bKNQVeRmQL-}eX5kKNd5&gho zdv)(*#hs3Qwg*iX&8EhYWl(?+ip2@5c_cKPihZEOybNRPm}N0W86OOW+YZBJOPX+@0O42Ay?sab!} z?AcsQjC}*32PgvZj#Q9q;131LT~RLj%lb(!cD)y7LVS(%>(QQNI9TP#Pfp~-04G)w z{pQ4Z+$yn)hL@n^cI5SuE_p@7XrA6Xtv&-~2`rQGh0bLYINgmD7c@<)JK!d3P38PX zBCL0CBb!^gDByC?+>Hol{zj>kf(3^1X9?c;3W8UicD%yTAlSC#S zONLKwH1CxpPFp7r0M?+dF3m3^W1S9{sRyi4`7E%&9wf&R@iI7KCuB*nJVCtMk(`uYFg+a6>9=(u-auCfgzcvbyiHIJkT6(0igs>W*Z4 zkGRo8?3~{j+pTQML9S5AAt?OKdrKjYd#9q*++Gy`t9&LruUCWI&E?f@-vC90$2WxJ z_NQX+q|0L1M{fV&wc&UTEr0eE(TS95)NQzIBCn4_*0uVt89vKVe-abS)(VFwQFPnY zoqylL-|93}gr{4{CYU=ZsEyfr>WjF5Mo2}WHTD)}THmd6oJx;7!o#j?;dF{aUURQf z@8G2v6jF&=9LxGPY&6GK&-6A%|dmNW6kdT_czby<5_*38EzsDf_2))F%ouF(H?39 zzzq(|iF-Sne$AWnYH(W7GB0jQ&fp-A^j&RMi1=+(KbcFU(c!auTo9>=To8-WNN@ge zvCk#Lhlvh6DKs5WS1*CFXSlw!;Aioxr}eUGM;59KcU}_`KHNp8miW0Z{aU^6%Nw9 zPdv1Jb;~7O8<zN6x{wAw$^Ytu6M*R@HVQd;K%(cV1%CHe!H2}JX>FLoTs0y8TF`rlm_$%~>~9z=5KUxU`@<)ip=L%VKC_htEz5{gG#YavlXobX&F61+ z`T2{#e(X?CP@oY0ctUZ{pD#j`l57TNRtma&EbrwwC-GKiK8 zaGS!m1Eo`v$sfD^-~WMK6i1w*>ew6LKNyYm;J+Lig5c^6<(Ak(8$AE;TL1A*5%xZM z)4{u0>k3T;-&f!DEhY=8Ry!pYq5OJam+g$1$PQswN2%Sq^471oC$PX3mxo(#P_*P+ z7g`@uPv=xT<=npin_T-;#pz$<+Keh7E!P{S8u0#N%tgUdib?>e5=JFF?%}TZ`+&b` znK0Zb7qcmWT&{~9->V}s3OBAEhg5A@tJ!iprL>fPk7qmH>g*vJervOMD9`9-16{e> zzji`vIv`B9Yc>Bq_UMK=c6UP<4JRuS(Abzx{&BIf)^G7h^tlQ^=nV9L2+krB9J>01HVR_=*N0Nd)-p07;A zV=dht>FsE))(ohnmCO?;S*50b4e{LEoDFPA5p=0cpsla|omeh(j4~~R!vAc|!402W zGXTD{K(fy9Bm1{r+DqXPUdxF(SPHpj|3svMwtJ=5A3Kq@RQ2VTGRxne9i@3(*0pRu zyB{cLcnfI#C9cyuBv@0?;x5eBo}z6n0`mK2P1Pg`=uM>H^BqT;l4h3Z>peNC#uZ0& z7BLc;CC(M-=-EG2UgL*@p)ogV#h4^Tz=U7`=xj#=(ktW3Q5s+tT(&Bfjv&An$)<`d zs4dw-!!9>t4-6{Al-HGD-|k?XI`0z0DcdTJ^(tqc2nfl8sEyi&_o^GtFD-9xn=7^j zmDa6z9In)QUFxO2*JU3uUZ$EA9cV3j@3Xc-rU@XktH)!%b)Vb9$R2J^>QEs!$axxS zeJOmp9gYzTEUu+w&1VyAyJMom4ik(*$HqGT8l$Ge#{!)Tyur6#txpaGC5^Aes#tV? zVGn7}uSxpfa0%*rJ{dU$h%8=@N!8XGuCASe5%Wzc zO5NUt#K>#i`O~)mdYN$k%QYDz|8oDakcS8n-O;bn6!P~03jxKnkR6KoRaV&aM_ImK zwRO{0PQ4|+@^kE+-!GS>K>1F(kMd*zurKzQnctc?utL&)gMGOtT7VS-ftf#K6i>74 ztz677&%x6vfPuD&H3SP4alkBG?H6X>-0&p^wXoKzG==%=k61mm+zc6s`)%C6!N|j3 zF0Ej@BA+=n?pj;>Sn<3szZF|G8?*Z|?JP$LA#&ZO?wsR^Ea@aRVYKNS4WPuHmx&0a z&^$iM`IOYQA!@}Cqjx~(FfDq?Uyk2NPm{Ofx@IN7qS>{Iic)$97wm5Q7Wil0?N&qw zzjoy{UGlLg5K8S(1}zd?uQE^JB~Y7~>$BPw_;w%Sz<;o35Zqfu)@5`q+OtX`FjD9A z^7ib(@Jhzz=8fW14J(GtxclH+RQ8)w55mDFqWk!aZrzeWx1S#C7sxNB?9EOT1lAa7 zNNQE1rw291rQnwCxZa76zsnX>tNkDa36By8L0ExqP4F`e3+K5g9J&0VutXhv#s8+hn`i#2@ z5=|Z&axd2LH=KE@zoPI+fg8$1w7$C#W<8!I>t<^7VjsF~(#%=c@#-M|jWCzS%2| z?#}t1_Q418D^DH+XzI#k7AysMo&#^KcD;y}ZxCAH#8Hi(wfJmB^dVox23*kNoNp@v z3Io8GbE8p*vij?F`g-8oZFY_O*C>aJ(~?iz&h2i`)`%n*4By?Yyw_J>DSeH;;^v=N zoxdtclZrPyinz=GWP(NFcbmylcKUc@t6CcLaFmnA9$1m>?k0U0yd$EA|2-uSsa zOFj<#{-5}a>^XvbjRr?&`w_U{H(ie*407M**o%w zv&e_(@T3Fj@IRs%QWt5Ry(EFNICxw^zl{t+$4 z!%W0oEAPG}`^M(?8Uas&ZH4ZwKLayDP4Y^Fpbnbj!#4N09P7i4?G+4}A=|;=GHAeh z3Z`~a5>^}P^LV@t1S*3N`UElVID(_Ke7tOxV(5j2*04kC3)-7qGvB+4Z>-q)tQMIF z+7gcrj~xMs{Bto!QU_F*zVEV1tudZN7EU|c!6J?)*!dy|Hi zPU@U_7x=$=0qT{Bg^?yA4Xoi3+0RgzT{|xap*V3~&)-C#rmhbKmF)YSYmg;8?MGod ztQ|VpdeM?B>}GECE$JIk1gDh-@dE2DDryWmJNMHpCqYIte0>s#iE}!Xew%qj;rSp z(BcN9fi}HONVC_}2R7i)HUTDzI1NyDDE_+z7y-xeH0i4akC?b_{OJ>lU-FHNG@LKh zLNYSHG1dk5qPVoaSV{S4gq{f>JbNKoxNduY!(T)zXZ;mU^KGP4PC-STHBJmZ%TK#o zPm$0*@$(Fp9TUTgr5oK8daR6@pMpep*Oq1b9zp$ib;_26Y5~DF0Wf!UN92pv_1F{*Nb|RJyr((Id$Jb(*49* zM!}IuI)6Xf`m>d0r9xv!X@2K1;H_gd?6dMvH~4UuIXOYZfe#{rGqyJ<1KGful=3$a z)_mhG?r(+#`8!a#J{HPIG+3_ODZ_8}PHhWyh z+sOmn*;}F@WD2Hb>QYxZ5p+i#PrH(~e3^7x;BhUFb(1kt?856?@u&=s0c0h}qu^cf z&7WdxV;zka?%c>cXAXTPNgSrlP1h1HVVt(4OIZByF*OR7doU#Oa0rMkLHF*ERe)FtGf5nap;;b*)`0+UXA zB;qVeIlgK@;$-GLHMqI%U(KC=TAhtk&0o1Q^VQYIJv>podY*aEaHNN>>L;1eU6FX~ zJ-fsECmQ^aUergW^z_=yVU3^rVjh2Fcm-?oK%27)T=%OrdrL^4Ce&Z1h)afTe(hT` zE|UCAodjBj+5d(^`r4i2>I>zgcRnrp1eVl|mQ`=%G;x?6 z(N?T2-tTNH9&Dl@@?YBpX}C9b6evAJ)2%PsadIK={ZPvCBk^Y!^Sj;r>wAPBUYiuI zjfFPSgsM>VBidMFN**@v$hHn2f~=b?&_s9clr|}q>48&QyTM!>HlQf5ZOdACaZ^ct zwYBWl*E}3EbA0Js3?Yy4sO39L?#NH|_?(;j%J?an6!JVG%JiX1ju_^SR?9CuB>mw1 zIo|D2{Q98cX6GWA%5&=Y0Uwj)SjuY~1WMW4h7Dc?vSq|BD%tM#yO#-`0%0N#eJ77} z!?l_XJ`lN2*wo~?uu3hwveVGi?F3lMah>-VZU8TXCZO^E2F%j((kERn3R!A&E6USy zmkw7D2nQxw*-fsWA&z(!BI7f_=-cljO5TR=1;J);tbp2Gy+f8^C}`-nN--bGIahgp ztma$jYixVBQkC+QB!nRpfoG_f_Y^plgqg%~ojNty!aiF(Tf6<>rcda|yh95|Ic+qD z72!b9%{*DWUkuRayH%OQ*kAA(5_hQZu|$rF+r6_(6qKmSbOQ6OoAmLk8|fdqF`TYt z7D!$$#bNWgWOXhTdlxqU4!u3A*niY5N?k)LXquk>Nac;E1p9q2ZCui2#gG<bZckg)Yd~uD!QvhxwU?b}t<2xM4=?Vcs+h7`*W9TU5lu$nixIz1dVj$? z+-15N7_iM>?XE_=`Ivau@8M8akuxz~Ts1g&Lqf8-d?#2&+5Llx6A`|{M&{a%w;76q z`Wa&5jBIDBBu>{On%+_5#fX**RYn}-0cUYryT?-^_hB`kTy=T9dg=>bsL5 z!?8qH=PaHTm~Gv$cM!^?(~fDPT(SW|C(dikSa(zelHy0A;Z@RfeE$M!q=J)A~En>je+H8qym=BGI$A=tn=4Mre!l zx2XTPrH@8t?O;_nL_@<-lGjs4(*@Pc-lOU|$ay*`c#qi@Eaq><*%)YV*w29~q!#2= z3zk(r)az9#(qxZg>T{*h?_wSH*W1{>rles0iH)n5C{r?ny)|hSTPU#&B4$}(Y)IdJFXkCs%HNyBW@ymG>>^8nTxyxdFk9c0Reu`lJlsN;vS3@ zT5NQzU$p$rFtcjq$8+0mW})lpcMqx?Iz(^r zK&KSnfG3~b`H1Vw@~UrLUD+S;S%-pPF&70r=I`xV0^M;k$9hhP=46LfC^&liu}jHK z9lNat21oVYa!n6FvSO?|H4EBxeL#%o=(BA}D_6~pm+X4_^KX|U0^3UW^E1#yn)0rmSJs>*gy#@6QCbxBWou-89 zz>~wlN#!Rjv=MUyP)3y@I4K=vTl?aYI!>K+dG7@2ba+RMDVf4MULTGsQs{Vo${Xg< z_hW08g5IHkFzSBtwC%;V>tnk0s*^21yR?Bx=XVPpP-C(F1bTQrh)8spZ#ui{j*-h2 zY^%$;!^Cx8+_Wk#&~ZA>KE)JTg8vxI^gQopCxFUbYCzOJnf21#Xc-E-hf-Sz|HXhU z{I3ky-9H$xoz!bDmP=|9aUP(VYITgV2&W~E&F~?8+K8t2x%vUePh(?5L-Ow|+$u|r z`qyjOe{B$uG?`s8`>yWcCulaZC7YkO*{!V3MB~nl9JQT!Q(5I#XxHP8R`T;y@!&FM zTL+#?3cBH(jZ+Cq6RmqkmLC?jL4qKl0gtN*N7QKQ&{n~YTDhm(>0CLLaQ9+bnH?^1 z4!1r7rM}5x@eGr&O?$56lLa*KTP;rXx%cHU2dU64mgzwb>HIK&K-}Zk&5+?5>kl@` z1q<(+fIwUucm0g0t1+d4JCEf2h~E0$nw(0)z7qmE-0KmNhTBe&vboS|%rJ_(aK|l7 z`k81i8*ZaNiiQM_LsqOlB?ErrtS$?C zfNA6yU^)m!!{n;lJ<_4v*SCYe`ySuw$5TFp#34QXlMSOK#be{k-8~7gS_FtV-7*CzV%S!;`goVu`$d zIdQkCw1=Nrf_cJ2rd2q@<5}M`m~Yx^?<1i;A(mmj=}U4TS-W(Wab%qjmR+j8;@fgC(QBP}(qc*izMei$iEE?U5 zX+H3z)U_wkf@6JJltb!b@-n7^XML#djDqryBRdb;dOfeOPV9xY4J5+0(C7K4SaVQ2UJwyS`;tw&UDfs8fg=%{FbPJXxpwu>>yZ z52ci(&LE2TauX!|(djF1?Wi`$@xa*q3RGxk7fBK{Oj zACizX;j|N~p#pDn8j`bwc*VJM5^fgpOytvlH3Zke%Xy79Xw`tc<$p^3kB6@J_br~> zt~{MZ_PAE~Ix2XUheAYY;&V!KN_STvvlmCU7uNKsP$cm>%GmE95MKoQ=blSdN>Q0z zT{^FR!3GimKW*6E7uepZBx?^9;}V-vPB5u7CdEt*_=C0@F{{*F? zZSU2LR)M!T;w9%HQiA2jsJrCLmqP+mP<`Xyl{EQu$Jz>-cr}B*3ph7o_t(`i&U*iX z4*ebIfXAEIX{nRQk8M}y?P~W`;h&aK0@gzCan+TZY*{LA?di485Gh9GmQkM5&ZL$cc z_7M%~t)rnLicE%Q2b=|-2i)E=(ZheD>;##@pA_P$Svw66X#W5#qmmOnKB{g{)&J9U zP-6VbEAwyEom;IPB%-YolTye9?S_d>B66JrCeB^fk{y`ig5FJ4QDXAQevtpHXiv{L z;w1cxSzq7|moonf zV<@KAk;z%LM9(AYa7t!Wn)x~Z-YSL=@QW^&tpCLP{WFxGUR(+HDZ4kZVd@{Yel*Ih zfbmkdL_NDPwWm)T9r2+k8^osWm%hJm$t^4~+W+Ra|MmdHOY}RN#{aI%En+n-Vm|a$Ug3hUaDs!x1=dE9t40Zlx1DHjZ*ek&;p_`$#98&Ha z%%2B%<&C!5trIP3AS^3;%X`vu7FA@2NNT&+@7h!$Kr-&GE}-e>GH1W6TVRhfcdW(G#VdLh#cFhXa-+YS1;fAXm+QIzJZ283zju>YihB#-So0@7%2 zrI;C$)?ednOg880wEk?j!Fkhg(0J_sRtTw+>Tlb|;fZxO&z5kXTbSK~Qs;Kkp%23U zIed3Dg0eU9t?9(Md3M7sX7y9d_ zDaz{APTj8xc$l%GW0ZZs$&M~l+9@@B;rzkEt>N+2XY;^+X`k{rADEn~7PjIH+Cchi zo|Hjt!7QQT3ruNspHy`(6d*i0O(QnB2mMu4b{Y;(8L>d4UIqDa>3O}cjT>FD)vF34 zHY3+O|D}7Xmamw*+P%JM*#A97gzH(O5QY-6uOjIJ?L4BqWSf=s@bIkspNglpN%_IC zQA7Emm!6ji4;4#cr%bk{yYFIOvf;|Kn1L<{`ZJFmY3AH?fy0>bmax>6M|@^% zzxKo5J3-nz`hAPi`PB`a3TC%7!a%Ir{dio6bE8etAToNWR?XOG>isss$YwB&WP@A0 z8JuqAod~cYW#(>y47TGH&aiEc62Mj^G%1TZy1P|%Mw1`MRQT(GLfrs}^yG*}D5p<8#eSs-4aWe!dFY#c zE!uSKdsSb)a*y+E8_F`6VS=K2!qw-qSEBvpygxoe-ZjL`hPd+mQR~RK)gV>TIEFp@y0ET?Z2RlY)HxYftq z4R`jhn;Xot&+pcjb;Ww;J@m7~CsO{fDa<6i zW6L)MC_21>6RG<$tKf)ZO}R~_M6_mvQEEL^QdTwV?`YfY(b;X|b*AQsZwL1VvPcci zk0LZGZ&lp{I_gSJ>SILj_U~&w+FYgSEV~h8Le`gRnctgIAuFpra3>-L?xnB=(^97_ zqI@?kI{;jRZ3#cyt>!zfolA{a0~uG>(5 zlo-OUOIOsmfij$9wO3Vx$`z(Dw&m=rKk=G$>3USH_74mrU$1NW#zD&9xTzhJuyI>7hjjY+_ zx@Z}oXp1`a%LOj^q4fqYA3ww+C$b28vh6^$q-mjf6k&?a#JtIGfF5&u#`Vibt33gA zEa7Cx0e7--S=bpekp1nYKzOa6;Cg@7>m})?&`NB1VxBOQOTGb|6#9T)lT6% zwpvcTkKkxs`mod+PKtQ~Nh%dIKYXDT&qej1o+h*(u!oAr8}BC1e|Z#y_XZe0OuU#i zb$3Gl*q7_NiFRkW624((i_Y97pjCNsxN_tMg$ z?5hZ6k9i(Z3)0;m{Pv9qpxL+Myh>mJpBDQ+I?F*lmrL1oW&oV>P8YY7IQN-EPeoieQYZkWA))ue^wGnq3ki|#H6y$%Bz{uEN!ANqm!iw5&?C$x5VRaR;m^~jCP4o=rp_DN*Hv}tc`zBpJG|S#jnLok&>-sa2O4<5DO3iw z<|dc%B5>Tb2_hI#DBO9tQJ&y%(a~JWGSA-Mo0{7WCc&vme8?V z@CLtH*rv}Y!0x%~bx6JvM5u{n>4Un;^NdXDszv)d-58}v&+cJ|Gk)6TERJ%&c{s~! zLOP^$M>B8U=O$^T&u~iP8O|T7r(T(3_UvSD0$=45R(_6ETauQ;<=8I$Mslc6Fv8@jeD9E#|0G1Ad43LU7^h1O&9riNlJ9cp6~1?F=69+@p_#FG zp`1KORc1^zn1tQmq1MpWVcaKA!26GU4$4tYM@Pgg=xN1pmbTrkoTN+9a=w}SC0y9L zlJj9P29*gCmDGvdJmq+|6<)VgesugKh4aQcSxl11b*Ib)8)<>!|?G)B;@Qi4dgvU_sxS6y9EGwXK; zJ=u8W(Wus9ax~A(GFHUG^q);kOX4nrh?1X}lcVt?`G~y|S2V+6^@l@v(HEzmP9BP* zeY^eK%rYtEK4s~jSVpb2@vL)hJa}>jB_DjJt z)~hg;zza8|m?To*c^6HZLMHt4%PP-=%yAn+)qwd^66>uwjNkrYHMIo0-00u7HemC$ z*TIK_apJu3g1hBqXhr$3Nz{&1Vx;8@xOU&eDUG#Bt7>m*8~S;FW^E+4x(`tmL}q#{ zA6C61QPw`&)5#+QgDLx@oAs380+t z1Htl@ZX&dNa&bt|&9=5vv}ydkLEP{&OJ%G(3As#HblBr)_t)XCu^eDs&K1^}u%P;#6D2UHS|E?fXrvKkj5Tlj;a}>nl zC7DE#`QMc%HC3Rn53n;`QZ#EDO&AF~= zv~UkAY&!fE)l7I}q_2?aXod7(a?Z^@I5J%4I}*x zgjfDDZM}{Q9`mpZE*|tkb$o9!?Wo^Y#Ug+;86N8#c(Fdm%NH9Nfj=DhgjJ4aau?NU zhi6c3?@aYQaaP>n%$?Kl?WS(CAACLv>$iV6!7?74SJLM(BU zAHvfFgc13Bzf>M{2yHv>I1`jFV(w?o?qKXsmpvnR&GIAn(ObXoKa^a>cD9p}RtLXE z;)f0HTz*PLsd)2Niz}dy$sUhRiKLROm}tsbF6@cvgO2XUPEJNNe=A}8Y)zjwS?hH{O__nZcF}0EsqAh{}Ib$<^QtfaoavrH@Q=ul%di^ zeMuV>nrBN8XkaZ*v5(cxaqZDYXO7oV~&sdY~y(}U2>BGt_ z!_=@nU}->wzR2jJEu=r6HF-3lZ?=4(PIAFrPHxd1-=2qSs|jS$$NYNi1CMo!T~IL7 z;mzfhAK!nakIJ0O+sHa3b+h#vaVIQ423wnR4h5Ue9PUfy3`CYOPo1kp|B>=)r&JOv zs^bK70;pe1m$nD5=<71xnN3;r4YIoOS!@b_^=2Lknm*Ewi-GkuO0t=8w9`zPa(m*e zuyz~3%LTjhFItq~beHdFvk$3i+*I)cj~<$J+f`YM`p?C`d)p9qpI2e=>O%h7xekrF z1S9>n^5PX&vM_EqsrzUYC?5l4q&S<`y zO(9TTF-0oT7a}=MauA~)H77cllHsY83tNiiy>-KV%5Sx*j`6x_>0 zQ^djzov{VU z`HwC^7A*hn+Us@R)8)Lxe0u)dx9D9{xQsU1u18jYMTQ$qA8 z@BOyvkAARwxM?78GM_yCQGZQ1cK4brLB=u{rl_0h00JT^_Kpf#H$-{>SL53YELoRMrj+hn zTSC&vmU0`iyyoucvyt{4p^o;M0DCupZW*V}R7q2^d+eukm*Y`;_cJN1bkM1X>&oHt z+5%nbLiij&w{!;RmSp6hKJ$&oqXiOxrbG|rd&j>+>PI9 zOYB?%nN0h9@RcqoUshh>{gco|IIXypT-|2S46ipCoC-MJtm;)syxJ!=n+LAmqMb(`4kX zkTc}pBiF&_y-+2Tb}>lr28^XC!ycifqu?Yf|D?}A3+9SqL1XxV#xVJY75%kuqet4S zT(n#QmSBJAN3l2VPiVdp->0Ja^7Rprbwn=+L>;L{_fLI_v0UCiCRdM^a~`-J*iT8C zyR=)snJmSmV2=$FZ?t1KihSng8z#w{sk3Kt-c*8PBJ8J<*7PeQRyvfNH;isu>}a#C zAPuSSEvOn!5*Hn~25)=23-m%y(yMnbj+)pUD7D^TQt*5+!=1D1QJ`X9euRW4AS9|Q zb+~QP^`|=mNJU8m87taODQ0=L{wn)?O&u()& z%)4^SY-O)FZ%BW;j^8bg+9keiRYen^W)GwUsTN7A8k$Z~3&*C-Ye!6E-nbqV8kla{ zr6Aq-f6Y)~RIzuIIe^$6&hKC}v#goAQHbegc9t~`#C9M zc?1Zzj$jL9nq)^4+WAIeB(2NmpAgTqnv+kn&Bxix`B&O$mB{OL@O{?X{T}eEtLg=Q z?|sz(MN9Z$2VED_kpqOW*{k*KL{KQj9h##2=hbGcgPRn*ogijCG>hGuQ9an8Ihvw3 zPc*GUybYII%40vr=<%lR9qjO^%#tu1ZCJQ0BZ}N!5T)yrdbqkx#2+P{+{Oz0rd$8` zqX+UM==jD!0~lv9Ld!LZzHyPYcH=^~Qb=bki!-LlJC%oIy?pVQb7DS{jcairLgbTN zt6^#?g=GTl`S_>gyN{{b+~KPe6o~S|`0ph~9UoLvRt=s<2UbR$m~ltdW}*GgiN)*> zWcQe|y1pFM8NW3G4~>d?F-`c|Z5#g|>wfiauqyac8Y0K7W+; z^~lZim%vLLai!XSX2hcXl@U8jGL%tEk}UbMTAm{4JYI2ZZLKu;Ygp|xGy`&1ssF2zdA)w-rWlDxBU8`{;17dSd?i~jlvdb4xqdY-~ziNiQS zPiA_95lwVl^sI?EZRmFBpH9?^*{NIdVxQUvX>zAs%EshZXkz^XjLFKcD;UCL4o(vw zPSWn7bl9PF7Ds(ue{48Jgm7J89k%mBX})m!;3^VvG0C{BlxD5kXtjLqSoF28NbMQ$wo z5}yzGW2~>^#HjY~*a%7=YzWk?MP!>jW~|l<))~4lpZRfYLRqh&18v<(ueGhZhJjb= zcoW9?UA^2g@DiR=eAfD08h508ao^FO3F+d1v2mrIqcO@4v_q1Fe zO(CHmY=3&cDuUJH)IaltK3PyqJYcw5TO1S5&7dFBA|}2uS1Hz^2SW+xu*tO@Ed@}A zcHH#XAXUjChK|9?`(GOCa6S~f_r4@`Vp2!RvKTr_QQYO8@68)7E`@TbKXpAP><7!@ zANc^qmhR%TgRKm@O_E$5lCiIzw>gb;%1gEgGXE5p8lDiX?nxJvG|OaZ7e?Hq#ww)@ zw~@`wgv3i*7cH(RAJruIqI?WXHQTx$0ntpG?4K;wjCT^U{ERPRYqZSOG4P7bA8(g$ zB^y2L<%!i=G;g^(`)(GG(=$CNw}!GQjnrVLAyxMy;1#)h;AbiHqbGf7dEsk7s@^Bm zi;t^JwV<+vqmGjG{31Sm*`tyYi{nA|6_vQ1F@o@s9PWgbJ`x5}hu^WdI{iQ_F6ghk z*b3sV*a@4}!5GOPse-20g6kSZn3ZNJ_-dChP+V=k+g#rb>w@^lN#Z8&B|dd~-9z(L zg(JL};eA1Gk%@7k0yLQ5ek|lTe8QNLAr096M7l@QCC>VN;y@la^cRohGp*}qh|UJz z)HzuPyFj4SDIz`LS&Wy$6$j2n`9ogiV*AQ7^nd%TpA8>P^e*ILlfZF2JY`Yl7_YR;B0dyWO5I&RB-1*hJxs(D(ub z%{JdlxnxmD$jCg{q+;4l_V1f*yJ+C^KBGc`c#tM{|204}gNBC()CPdcRcp33OzUbp zD@Vt)pZ0IeSM!^Bs5$h+dqie)EyJ_E{7R@wa=txin01WyzDrae;CjWoOsWnwuU-o? z2m{I6u>`Fee2G39r2NNpV~yr;$3N@`rrZ9n>3-S^Om`}jn<)T`Kb}#nkT}PGv}|QK zx#CBVwh*dX(OS!)eJqB6yU0Y@ZCL~{kI4h>^8UA~q2oVz!8sKxJzMG*#)_rBAaN>p z`TD@BLv4-obzW+L%Mq(qSBb`Txz?{f<-B4}B0Nzm%7LMgX_}?WIGW`Bn>^l3V7-wV z&pSh`tox=H^QU}@qR*w;^I0`LNqsv-S{nD|=p1^GJ590G#R6R99AR|XkL}%h)tp(T zq;|3xb!75?I*2=CddzFdh0r(<-DoIcHIvE(KTHky`<4BzDg{{2fYEe4dAuRb^j@72 zw|e8EiTiwTVZ6YkGC8?1+LrWrJU!{<&Lhps;!Ux@JVo|8ec9`4AzPj-fL$NEtMA!H z5r|W26o%5DLx(Gx`0m{J)RuESl9)_kV}kM0+v}0KQ*TqY(7=HR^MS>Nye@(?8tz|; z2r1U67941h32o&`Runb9J$OyV6p_(8fA(Avj!j!H1%7e%cyYD&^jx^5%Rtw3eh^QX zD(IrC{Hy07{WjJMedf$q!UtM&@h7P~U+#PP9vl~mTe~wzN7Y}rkNfba##hHZPuqJ) z&P)!tXD)~SnM+}<&tpLT%v#=r@6Io2bSU8wiY=Q~J zhF)EGr`J(J6|&@Nnw~#)bUIM+g-U|0z3zUI zzGG+S!Aj%+wQ_rcT4v&2=XTBh_k^1AsLT6jehCGJZ1)7{n;ZHa_Q@$Hpu7=2ykvLW ztfe0}iF$7#n?f$n6?LkUc1_kK1=rL=`o2rd%bENu6*{oB4@IR*rzP`%e_@w z_rqpaNZAWqRp4tB$que6E#+i&X%g0*k#x!D_f*JgctyDklQ639E#7{R;VO<7?Mn=ircxEVX?%o9hW`e3Ea10cp2_ zYR26;5$2Tn)f`r?b6I>^93;Z!du%&XL-G;!D&D&^L6xjkVRiZL6c@9m{_!t;iH{Gq z90}XN5w2xcous#X9#&7>jY7$UcNM*FUwutwo$X+jKQCSKEtvna_v-VIXIqO-i@9dz z*pbth>-~+(khY!IKo69I+q+c_1R!ZP%PEC4cPh_D; zE7rId>S^|D_u;ID%S#Y*0N-=IOCs8nptWlMeFrXvhko_g-tzX}`nznNo$e)erX*Om zz9{?h^zJr3M7@mdil>!%TKUmuj4v_zi&IfkJ9>2~l|Dv`W2Y>gB4?y3!-$fD3BKQ6 z%MFY-$F-?55~pwOoM;wU>ZO$Ho@CbZYKDG7#xHGlf$U#r= z?5c6Mxr19g4M)26M>(Fp@f;q<{gtSIABH_H1f z!V}QXQUZ_gYy*?<>6p@P732F~sn4sa{@CYMRTG$X!t&75a2{PWC062NjJviowo&rh zIaVwj1|b1B+07(UOcHL4W+Xq{@%Pnc;g$NF5g8u4h6S3SP_S=xq>fgBpi*$``9N{l zH;{3~#nHWt!RrhNGjOR!h2;6N-i+_Xo8%(;NU77r&AQ84XQfWt0(L{Stm#=;wgudu zGs2E``D&lb?$l$L&|pq`E{ruNjqX)+7xEptVe=mj2bcaA1)@(SMWy$Q<8jTsX*&;tgtL@x>()O{KO**V zXUDsD5p9M|#RY_<-si-E zkd71vRpK@klerFSn2e|GbW9VjME`@Aj*lBDP=i&l~w8^Iks zVEg2-EaDzQA&FO1xU(Ivy~}O$BfQ`?mUp~q#S?m6*7jY{hcc!n_nf}q#|IS%vewho zsASqtf$d?FF>vAm@-u1Dy)#(2bzkrL0$by1yiIN4WgkUsvT z60=pUZ*CxDZHZ?WcM6kg!>Zi`~&KEFLl#^0DH!q$V*b?z1sE;&^6T}XjTzi7g_es)iC)`h}$JmVh^6Vp-24JO935CvSMV{0_DwrI-sFh#Y?04{N zS_GZOdUB7qW3=-zxMwqRtj$G))5ixdl99+f{*o#My`-XE^8+Hu&=s#5w(0^moUg`T z^VZzB%E>3nryjCDQio5{meviSF ztXh1N0Qv=VX3iho=T%n>9wIBeOWVpxQTM2AUNjINwQijG9yT@1TG6EQ1L3@X@p|&+ zbmxAxIwp3fSX7R$e|8dF)hYBXf=ry{{(H~jZ!5xqL;FnoiT0n^)EAX#7I*W?OiONfWf4;QN!9j7vo3MLpRG-yTOwA6 zZfsnaEpen3s75yfBpR$Xs0GnZV=`dieR657`tuRTV4aFKU>{rWG4odLy9{Jp%!raD zFNF}+4Q+SOe3rJtd1az~*ml<;774sa41&|9D;KlF+WY4^(tOpd^Ti+EdkgOLls!+e zb(?%Ai=|gKIDV%*NzcVt^s6;W${NiKVF(?|p<9=wbjnS{f0&{73o-VcC|!q&pmF`Y zE`$~dm}f5&{YYowW(FV-*n97BCA~KOw9V|ILx#Ph2sNLK?uIQdUXN<7N^Yby46fWT zZvxVY0{JMVd2z$?0AV^i8P&idVGqfhvh&}iVK8m4qxSNIu4hYP z-=*X@s}vP;u{4D_NwV-q&7HQXvEJMmj&LkFUD~kK=~;28n3y?8yz8iWo&Rj~3x7Ss z#zUlVEX+VWSb9&J$0LBw6!gZM8DaKubaT`I0Nq?Q*rFu=DXr6b(2|)n@ul&F8L{iE zi@)DzMd{3)t)C?Xr{}acf%=K%w%gSaRq0&T2q9Pv3wDRN+IA(#8smHL7{)(#p7md0 zZd{OaN%hP7I88`&e z#2p-ZReTS-GHm%cs0!QI6jnWlZp(0nOAe>b$OWaC^giW9(^r@8n-L#RO;cao=yNpT zWCx9VV7Rt1Ed&qE2vw>D8_2TmxY%snABUG(viRXiotR$y)M;(wMR+*Z_^^Sxv~(@z z$!W!KHXhbwDvOacnec07w8w@|sa%EUyX0=@#Ad3;6-pG3m-%8ZiAW-}DvO&V-)k0O z>)tEAFoeHF_WV>69;*2AtV*pM+$MXMl5BvY=9+l196WBQYYmJg|jM~TN@&|sNEz`o|-OEOJ?h?)q-$Pcz{o8G>n??uQkvQe|kQMCkb<<9^A_!1ZZ&oQ`l7%nF zN5VI{cy`CyDMovg?pH;hnqAtMY#ijAEx<@gJY9ow6+v{Ied4p@GWwPGIgA3NsU1va zODi>GPF%-iGKeEJWJDE{K~Ozz8eUtDj-!w-e2D##X#(^ z?@E;;);&KF5&>@jg-{~0?vzHK~F3#0yE_=gQg zv}MGwwTQ_S=$#^EvN)Y*D&5&r6L(65^(wRLE7P0h*MzdAQqQ}2>QA^Mr+LF$^*S0; z0G-{gH|t6K`nORciZxxC-a>Iw7}oet%0b1PMdytxdU@1fKn zj!N~44j&TPV{u^xwJpaAar)V@kRtke2sog)=>Ed&2-KTF*_gk}*ueuiSuJ{`#}b;h zrp!_SK$H=^)GN}}E_&*r4ok$|T3mifF(#+-SNK=r@K(Vo@iAJ6F|brW#b#HMSd|;OEaFIpszP44T%&60%UYwIBbi@Z zN8kF}F+%hqxr;Emo}OH+Fe1w{mZ_Z0L~r&+&-g??X?7Q(9|+-@a@|9RONL284rFuC zWrgFLSzl>^lO7U-H|!?Qmbtq^;NGX~TtC(_NF zRg!#rz+pwihwtREBrJaY3N}Z=B)a*>12%;DM|y_^T_VYP;f`9BCXzNs+3DmrNQcg8 z{}{iMnS(fbE7LLMR&1QsTOKEK4zR)Tsmr>>1h7LytQm?To~p%p$hN5uTVvrGyco3FK98da{6IWCT<6 zg~&wVM=|0l($LzuFS0Nf(p4MDK|MPz?{480$=1-U%{$xK(oe2U=3gm#?2oUp2 zvkSy@B(#s-RT<$48d75fx^Ef|z9&6@K@e#z*0cmV_A;Otbw?-=^zs#@iNO`H#+RG% z-I@&45W4QTcP=F4!lghT$_VsdP=cOSDV0Ze7J*jQa{TV_1g)M;QZ{eKrimH$yuQj{ z0-;)1Hd(9)chrWWrij@uFYj61Q~K4uLtprGs|+H)ntRQ7butv|5-1Oh6Wx$J>^7Dx%* zk!J_3Cv$H!nIm(pZfeOuzCKI!uNt13*Y)^0S0>QfggeB;BV3>sZ@Fc|d=p45*SsFqz>`>Adb$`3* zSwC$*nN^A#5&~yU!snIn;MX!r)&@*cH|pY;EGV-HQ%kr|M)J@?hZVi^Z9(L=CbO(D ztmx&ImvJ4n-Qg>RZrr<3{dNBEZ4tpOeVQ2GwtAYBf5F1lsXj<-Q#p_tNYctRYe_*{ z_Z4+;#NY8>})_P*5MeEEEgTpKz7J_AX>H+>PY%Isrbbf)fZmX?shJ# z9%q;AhLTd`lvYf1>38X4xvfExD0DqEKgadj`hGqsP~_~VFq!rQGszTBnDE3dBnB2g zv~M{c(MJ@9d+nQjwWdkgGT6Q=068J0E}-b|FZ!CBHFwQ3(A~I>cm4oEe}oxRaM-5K z)Itkrf%f5PT!e)k7B$sl3M!^M@z z|MVx7%>7MmpE^Pw!8o=1%@Og}E{+ZgCEGyvm4Z?Ht$oV-a(5MY0zqQ{+X@5z=5}@* zTnlkXhZ7tODxOdc9;AF=q)pjMw0!~ZDpIjvMr6`LBVHh{=FKcdj_$G>o|| z*XF;El@(+Ieq3q}FxiVvB4O2QdK;E|bKo@X`!_^AQzQM1YHTKaJ%EAdnM0K)(0V~4 z;*hhgvxz5E%^{2BnWidovh!U5Kzqfzks)y|ACz3gzoX<4?xMiKj?OERYI!R|fdi?HTM7!m)Lz@LV=lBWGyWh%2HbXCbxDhw}6piU*= zI4vhshbYS^xk{-=?!_^~7Z6US^>r=nID%a0#b(k-iAH_*{m!JC$NH;=x7sbT`khQt ziI9&av~5rI(s5_LPGjwCkfVpvrUypglcex)?DQcb*ok~Q+8omLTBr630n3j4%(^(5 zc_SjKtL{K$JNCxi1t4EFTlGEXH!J-sLR9;|Mu<8BKI&JLznCgnu}2ao8jAb0r&;^g zo4&TcEzmL2f_ILR>AchU-p=(b=NnQ6L+S{RJ+qiAc(qgfE4%(lh_6G^6FF>n1s>k9 zksQm0Ql3YWLhxG6C$_QsJWoFudALew|M9IV(;C4B%QRt+nK;mNC{wZtqwUCJy6#L! z8N$6yN0JE*{DL>4Z2dF6HWEvz%Z|gyy3|((A`x&A_5GPQ>j+drfAItNG2`|Kv1R85w!^INjt5JaC%kGB++MLnQflWOW?7tg$!v~$i(ytu zjraU1-4z0Z*)n)ouY2mG6JB{gprRmy+00>%jIth)Y2%c4$0C1+yndC;``_XlXz4I9 zE5SI`ny~B?H2}j+reMWjT`YRlA9y$E-|;TzGIrr-UWpE$VLDU%C<42P5|z5e^S;Wy z;(YFe&*zmNCERv@o)t-6?IR=ZPvQ@K2cu%4@i@b8W#s|eSs}EbX?$#vK*FK8Ox5s7 zVT%L1I5p$#nx3W?99HGeElg7U3vtUn+4RYp1LLMEPG!!zRM|aG+*1oOUXdG4C(sL` zeKI0hTXD&c;n{{~NGYa?W5MN8LAsO2S%7=&nKmeDuq@0mg*jcJf+vP~ef3bL@^EM^ zL`w(5RIRnwhm&esD2irNkwT_@p5@TZl7%6D*!aRofal(xnWpZ>{>$G#q0g5rKbk7B z93dfPZX6|pAh@5ch={ygbA$AHzbRG!;Odkwno@?LV@LcA)NRd5lIr*?^_bGT%L+A0TnVFX5xoaj9N(sBqOIlH*muMTAwGthe?+pg`i`aFzq#vhETGtL$r z6v;2ukCRb-{Hu=6GZt6mW65Il$|NOorraCv6{mg~ISLwk96iu26L9jpo^@l=>wWie zOIpM%SKnf9&vcKJUxn`Q#tA6N2yjYnw&f~@()K-mq-|^yWd9Z3bh_@QQB*N1oWS`8 zo(~X2N6Ys}c#qF@-Z$Ber)2VOeMoO7ha`JkWy31iAn=Zg*ZR>FMZ?_s&8O zD67Ond`Qn_sM-dkHnTO{PlScjh-&207FI{Xu+ZPmO@)`>$W{6D`l#2O33ob&co`hJ zlQ2DA;9d$iDUx;1T|yH|kt0APddAxU%fFed21HEbp`4xRHy={hL}5593gK5ahjg-E z;>JttwOTzSf8N9Yqyrc0z;O@iN(#`z847B!9C5Oo2@= zU^L&smbSyV5q!W20dLs^%WGIyB19N}ERq9^R4D?Bx}eG&z|sg85##^{ub9P0&%k*S zLNdfmS-Jc&AEXmd`uZ{nW6BP8%ZQM%o)BHHym2d?Hb`r=?@Pga#<^>^jVr8)K{PZ( z+k?exStPgZ<^>(v7PU^vqHK1rX5%yA{q^xl*wzk{@Gi7VXKBKbgn+UCUp#UTL1wT> zHz!rd5BOoZF8M%}p|?a-6_BUzH2aR{e}~c*`~5#gX;1$TD6P*zs#qIeKFdyC3bVhJ zes@R5_C_3iSbMl_Xg%iBHQ}`_99QSBoGl5Arbl{su1;`|*GK_i>;(_~JqMc3UL)r1 zvoX)|oq7dsw@gp5dPFc^PM$JmDt3K<14! zoU=rZNE>t-L2J45F*G-I|1*UFmAF2$bVoN6M)cMA;bUWx8s3F3Io2-rYCyx3c~Uht z5fSbrHw+?cv|qNviu%T<1r8@BU;RtBKlAS5p)C~1CVht8U=c8Gjh$X8geCLg%+}6( z*70F22edQm2=<-m9d6pkQDB zuxfCF3wa0IDHR60Qz!3P9kBTp(6)=*(zwrVTdcSNj1bnmz8K_P8X&hjd!Xzx--Im` z&rs%@67qtxBNY#O81?jy+3c^X(uIQhZFO^a^zM!lf&K8ltC|74p|O3dK@mW^DZK68 zUJ7u0xA|^N%VV?XlOz!nKKs;{2+0pEx&p0V)`VMc>zV=V{2pODMG1FNcMl%)v{{vP z+3;0`VanI`dsWelI>p(KGWZX+7LS>T_{P_oI|mDyl#y41jHC&YJ8yATNx>07SF=8O zVG-ES9^qZ8%hXjKoMNqWLnmSRCXF7#!18dGLs_xQT7V0J5byFt*u|R7Yr3p+O^CV3 zfeCLx2U^Z?<&MT_B^yCHG;z~s-|=V;Uyk7ixxb&P7m)x}LqA5WHo@&iJb{$7bk-{# zkM-?%iToJ!m18^df^AGZr8qWcN5$*IE%iQW>&E$naG^q7p+AqZ`(a@G>DFqvPs+&s z_u6>9Wl3+W66+#!21^w+BPsW#W4ZHkrq@haCZZjl0h@?&KT%VW!u>!i`eS~&R;g`q zU#I1|9SpgfI4B>7f+k06TO1I|_-4eBLm8;8g8%2(w)4M_Z5Q)#dDr(2o(=@QLKoiW43)4l;&wcnB=VVv${QHL?CfRFDy z&hFM3B)Ou_;mtO3^b$tF!mVQA=>)I~^25mmcsN>31KJ_A`*H3Q20*-lX?>0^zQld- zt*|g_(nJg5NISR4S4P}t?h|CxX*qQ0G3C#jjjqhC=S;ls$E!cgc0zEq+JXHnZ8tQo z7{M!?!vr`HH7>IXkQ?5ePN!ArOa~Xa`iJCFPMbNJfcg;v)!Su+-)%o^wYcBng*LYdySZ-BRP$coNzWBX&*{c|PdiqyhP5X;q;|Vj~bSR2A#~b$us%&6oa2PhXF^>E1YRY zH2yx(y``0;R6i99Oi~9OBYyDT&^Dc)O5a&hq!}(L25?f?o%O5|I`GK!$)g)aWRUw| zgdMED5K`k-0t)UB#d)R4s_ZeX+y2RUz5L6b$c~s{KN;^3k99rDgx;))@)m5?*dA;I zCN!{J3Y5RY&Uk&~dCLAVy6b7|j#HS-9kZP~&byh`-tyfa^NaGJmr7QgM<(q<9uSmV zaYCcnghzv;nZzG)rgTP!Uqx+?2^~=^LNM$&^(_C8G`ulp+1J;|tJ@tcYev(|`k*>Q zDXM14V0@`s;LJ5yX7?em9_DmJwiGhd{_D)wZb{(%&-}hJ@&qWd2|~uzMT$Fm!cWai@OVA+NR7>k|%hky?mBhlPI@? z3H$z5#PRvQxow`GFB6U6_Fm|@tp(N>2P4AHKqDkooT0Y<0TH_}T-+~8o7mBqkRAN! zK#FQ}`h;J^XDskKAFO)uVr=RS=(zG`P)|WjZhJ0YB)7v8W7&7+4HSp{Xf`ze^__Z6 zhoO|1ig`vdBjmb@I~4(LQu|tQkk5u(`vcskttb)^{~w|Q^&&_Bd{>twcl)xvoh6(S zSN`w0kfGiAAAtIg!N8XnexAq->2Ly>zatHifluPBo4thhJp?=cwgFMyPZVf!zp#M zcCYdK$!u2THoG$*K0~sKH-mkcmx!TM_fO3l*-^3={Q{eOR&Ty4(okDBb8{=~G3gcbA z>&|NK4$dqs%gwk8%&L!_XB3p-)qfVm&VDi)<_h4~RrOH-#v&(oE~HD&i_HS)pTQdp z{{*~fx&Il!tNb4TuT9BM#(&cS{AXbA}JWYD2}pLlTLW7SCW>zE=xTJ9E*g6fq&su$^JiRy`gVFZ$jX0xx_ zF?a65&jzM?TiTJ6Y2b2)Z{Y7PUTR;ykc(CYX+AiyFO?IFVw{Ba7%hHSd8FPD8q%a76cS09fU6RPyh4(H^0&ZP_ZQk(mtB4WF6+vAsF? zZrI;jG75lPG?`57wI5SyL}IFPY>baT(d_Wt9wcU4b&7c5j>a?3o18*d-g~0W`ws-% z;SEQ?R6*Pr15KNz@W$J4RnR{P|67*_(GHU9`_zZ%82 zbj5H5!F|k8hp1AAkC6KX$w#>i+xkb(d_Syu!csY8HcoJJIcooFArWO+j$t@DGsqhy zp#5sy6u*^a{1}%QXk0s=kE;nk>~1PHQPO1o4Ajv0x*3q-K|`=>PN>xqL-)yRZ}}3z zdbill{(w}Gj@OgWlYSAPy;a5f+g*O!TCVCWBK_acZ=CaQ@C&ThW=NCSTpqk^9prR3 z#2gR4e2u@Dkcq)Je>-VCTHg`gx%}{j+v~d{(c<}-rr-|Xolk0p34=bA5gnlw`nzGJ zL*U}kMpD}a-tB@_QSTxQ=bQ!z6LF*s*L%q> z*t0?y?yMvv~^|^spL_1Pmgr-sdSsKih@P9yqv32KKm`ZzjtkFBl2)c$P*j%YS zQ18f4*oj=xT@ZV#jZ~Q>IA=osIy?T)aqwHU{}v8bBn>u#f1fy?3yy5|o>wd+ zhABLaY_6Gs)hFd}GiA=XT1doH=t)3MDT*x3tc4+mIsM{+b`tO})zrcSami*HrtG1h z#OP#q(v3cn4`K6<<7s@7@OxPO;=Penxn>q!xQqXRTO7b1gkjhBr6WbF(gyY^dK2{p zNzyMeFBHOimcb=mt>1Ny6Rbjix9a6<t1Ba-6HlQUI03JpL8 zN5{yu$cM9P`;xyurGq35*nOUq?cg+8#8(W1&|0r>&XKh*qXnDGMN;_O*SzewNDE4r zLi&rON?6bx0y~h^a^$LZ%6d!Zb?&3$n^H9%UJ^Vb$p$&p?0Rsd2W2GG_*}o7kYTa% z@*;I2ArXDtF=d;nL2F7f&+ScHcb%$>46ljQ{?MT{3Mwm@@ad~rO2z^;>BZOB?6^&+ zcrVBPYHwKWybP8=N#^xkOl1q^bDF;=YdbxPWHl^o(zm_Jdf|4lqh`@f2Z!>#|n$HPDVxAAaaympv$ZAl+iUF9*W_9++~fRI6$(im&ez`3ae>@ zFNBf~q~0cr(WVe(unHYuRB<6#V5g9EZVIQ%V5`s8-UqYo&eD&lxlnmm$4&b4;}sw9 zByI3C_=ut234x|%ZgehLeby(EB=m&jQcxipr_dxzn=iQdwmae^tds^U@R1_M z*-b=Cj1eDZP#M1X`9UOPUUf(&u-cHCR`$H|)j;=x_-$=+|B|BLR#u^G0JuA`?T{1Z zX~AguSY2hdpCK4;*$E%OEx;eR>$FfX97R4uH$LR^^@p&>qeFlCZl`}uHDk9xW8$Aq ztmM)&m?uftZdFLkB2LRl16{NyXb2n;)r(6w{>g{Xj8>A%pe1#huo8IeasD<={XVam zznVRR!2re%V_RdEE=o(iXg}iPY@x`<$M5VQUQixgl9{f|9=FMhPc&Jc#F~|8K|S)y zjDNj;W$tJ2`R51rYq46h;{fp6e!!&oM%y+9XQ0}mNVsKbG8Y_o+mXN9Db9?C2nU7q zQ(P5@(j&V@nLB{bD)%_zZGvOse4e7TJ9;@YDj|X^H}mA?Nd5mz3!{xvU{kAzzSbKX z-YyfRh5KEW%>i%Rghn6hk&jMx1MZ+BeyQ=oKP+)uPY)(STkcjuD+=B!X?|ca@X$D0 zy7r^oeyL!nPAdC747Se^@Pa$8`V%8d37E~tTrrMwC~K;ZU-{^oqQKHLN)&LYBu?fa zH4g_ht3qDoN^4UlPn1@Pr9~#OM&XwgU?^I&=FPcEU-b`s?%!|i{kt7n1=Z2n9*F3F zYKC~z21c(rNq|-uZ6jU_PI92#&$g6omXRghmoO@ zSN1MW=2l0UbAV+XKE6#SYe?;PLsb~;pacK^f8V^}$L2yS&2KiF_r)qYh%z~IgUD{Z zW!kZX(I^VE@ z43sk@~FEIF(0pf9{YPCsqmI{3LAss5DL#mO4!N!B06#>%`R zdqNXWQQ?<@h%ypw0?jb}6)*+AXK2a!iJ9Pos|my+{)qVzdNHlb-353tTaKZAI`j_A zKUOSh^DBWWfJ4Knw~2e8J3C}2E2j@=B0JJ__Wje+8~wYR-^5cgBfANM{VTOs3rDT9 zA$1B^b}f_i;&>s`u)^rMcoLT~_x@ZIe`#ZKY#?nhnOdCT?!vcUx;n25)!Mc(BY5NK zd?)J6+(rwo@K3gtHD>1|o3ffVW}7g+%PbMqGw)_x5r6jY9)Tc1vbyUuf@t;Oi(HO# zB-cY!ZaA4ME!{gf3EibY6axzU2}N09-?K+H5_FBK5@`Jw*#T{wf^RFYYE73YR~?&Kb;P-gGs*hS5{~pSJa`L#&gcQ#w@%QPaDrVmr+*#>8rX{_ur`6D-AUFm1>Z0_eMO2 zFX3oZ7YA{k9fChfnvM08h)rEHux~gn^2;6226npUAiZj(i$tcn+rkC}BLyUEKCZE7 z@!|`{Hg`bm=p${Qm>D z|Ca}D2dn)#Z0Vm%dRbUT#PP~vjTRL1m^if(Xfj*vr7XdA_=aI?xI9ms+*tt+O0GUv zC?_!6dHx1vR~XBL`Bee`$|*s*kG0hJGoo2tX}#+==1hW}A zjf|a3&GGp?dF>ar?|#Z(zA%x;G<66men}cmm9WO=Kbaj`_FEbjxK7>S-x*$taWJ>* zRR*_lptXC4`gr4ufC|ECB2`1Z6%ycrMi!HgEAsA4z6*Dhra*SEx_N@6pfYt$(X}hF zm-UA>J~smyUa_xVpDqWW|LQm;=Yv(n+IZ9HjG6v&&2nd~yKdUZ&V@4T!wkV;oK@GC z3D6JhL%mdpcmtmc&Ng>#mPZ0hj1GtQel)I^(;3`(c-C%k**gaDDoO{cMA}2jbY9<) zs{+b1FL+I!6%4Om{rp_te$Ln)dGG)IN>|GAQ)oQrXm9F!*w@Rv!^?5R%cWJQwu9q57aKt0oo;uwoY+LjNOQnq-3d&#D%tW+K}AtZ{9CV zN=@fi$@*(K!>=4;N4oHGV;pV%}ww_YY*XSBJ{E*qh6@YLFdaY3tQ|ciwd6r-F8DP=w zbcL=sWNcRrynzk?O9hf&mx(0a->csuxK&x(L}(HjQo7Dy3^%g)XT-zOd|yKoHE-XJ z4M<8tC&BERk*-1hAiYoq&*ucNGc1W6q^er;NHxQ~4^Q}CrC-v^$ufSp$k{FxNRI^l z(n{pKrU7p;eBFyI+LfHm8^itvcH!ZY#`A-1O19r}2M_ZT#K*Ury*8DyzAv0&)4g!J z(SWgz_pL+*WHgpoWR#d>Uzu+E;L2>l=1<#AW~b$~1u<5S_C_{a8x6(y_?~GI|UYPMRcU;tabh zd6kC=WEXY(x3Gh|Dc=Or8gtTD(CQ>Wc!CuPHFKZ_uxItx>A#FZE7tl;Y-&iGzcwW@ zP=y+~dF6_!80qBRKJL zPxV~3Qg_on!Ey2++2QXC8z8V?fk(m&Kv}}MRA)X?kkH1?Z>@&wrhzKmIwd!(Q-?$_ z&NUvFw8$e9hn_;A=fz+)h0BWCGm}2FO1e+1i+s286u4cJI!&X(tM*U zKakd)Q~Nxx(OCw)%r-Sm+f`AhO}t@e8g@4A#yGLRaY_6B;IUEG8(lQUGNoIPIQgj2 zl`ZGw=)l|{q}?6bbhoyt^6~pToaz^S-PWYL9J$@|<>hH{wx#wFR6{fG?z2(e`41_o z2skrU@yBp0z=fPT>`-lnWn4!k#A;{nuGf5)suwjZgexw7AJ)`A6MEj@8=0aQZ?b9i zD?YF{;Fv=($8mx-m4tj)Kc#skNz-6I(A~e=N);MfE4vKyVOQ z>M(Va3?Y%i`kMy9urxl*iXb^xw{;W6AvHQeTA|MnfygH;???1*_!x&%Gn1e@$G< zShtx{3!OZw>`^M>1$Q_17<5nn4o$7xmqKog7|-zx*0^G&>k#lo`+QV0es|UGT;66~ zRcuP~p1<`QPdCE2n<$E1uRap~a3--Ep>|USKhaoqjXcFYXG@{_*iP~zU9n!s^M?2z{Ji9Mx%6_=cCErWr zy~uM3^prge@0-R4$a>A!RVlLcS?m_T*8m+vTKu7HLbj4)3}rxwa{^;B6F}fOg0Dda z8lUsL;}&!*g<>l*YnH%^EH3Dk+z?)*tl zHZ3Apa?S%ZiIrSebI(_btNI4Z-6cuVz^ZW&Orx!)LST^ zU86Pd#|N?zj}y}E^{XD~;volyKhMK>p=g;2R&$!wm382kWzD*`7*q#3BVGZvPC%3o z4TKf2R}IA>8$7K{aauNLlw;NY^aUOAU0bsWQRpk3m%hytGg>+C+n%_fhqI$MoR655 z#0io4Pw|J33YLxamtL~a7Pm*xG*am-k5=*Vf7%n18F5nYUw`!MOP@7i@wuE%oofnh zOR2B%u=xPZ)5bYf-qd#cp_2%8AC^O5w6hm7s7>YMZNm&pj(!rp5no} z7cbwMNs_VCmcd+N#~rw9X`)+kDkyS+_!|)-YyDX-@=r`g(whMS)0qGZz9Eu8MV3h9 z>>Rvm%M`8&(9h(Gx4W}Q;q-N_2J%ERmhi#dre4Nik$omKtltI7yxUxj2`29wA6R-j z7$pw-++XQ@e}1WfZqw^1fVFJ+j#FnlG%X&Q8898g$iMbTgTTrI-Jd9}d_e zNxRR*v~OLU%oml?)S1Pp8R+_E>G^udG7!}Cbu^yw4ZHn{>*EjcWhG;g zC=8L7583UjK)g7vsq;k0wG8b^nMFytk+JWDl+g3BCn*uAs4)_3R;}_pYF_pJW@wJp zXx4^DoqddmYsdZt1Y0|p+yi^PJ>}|MT~3tMqv}moo&Y)Z>)9#_EARte%2U)xXMsld zKC5Gm)>o0G{+?A8gQ?T~f@VWhOo;rN^@Wo)8GpZB2nQuAFErInbg8CAV^v_8NoJ^O z?Rrnv)ygc3xW;az`DhL7Yi^@=%REKVJ4aFCBLd%{KMt(VX|u0Bm3BAg6-0y6Tve3h zf$<~@ZZU%fl)wrjPJ=$ls);mdpp#knV&Az&locVrpQX4i=thmwesTOnz1|fSSnW6l zpooOl@laPGBp;#h=|KqyjEa|vD#1ThQ4%rBOJ)EHl89ft5KwG}ohLo_e8L&A!G z2cs)!x}cRTW63Uj@BwjNr4e*xjtkttVv`PljvI&h#)Ku!9;!+?KN?BKQy;&JdzTJ_H9T*WY&URg8Ig0}G zgvc-Wv;;HGUUfOIv3Y~q`*&U=)SJIXd{8##FLM&gv-bf8$RNh#<@>VGKhAAzN{rvV z9dme3R32pTK#M_UjY3LI`{c-xoTe97@Pt9IDTQd}jTp#1anTUgx6q%8%y)^ePJHP- z{^Vqnnc9ZBuXXZTCIlE8TS5wyoMz+Z`&c#HjG8hsy3f(=5bEo&F0A!RAp;3QP5n$L z1KVs%?LfrcomZpwS3?7VL146Sl>3kFNMd5O0(`S>tmh88o4cn)rSed2g)P3(9b$qGccs>=^t z!CsE&4Uww@DJ3!EpmcK1k)EIHvuh0oq08c;*uc*irbM!~axN3~eNz@{vYEsS*o|A9^fNLNWFnE(80rxszGM1bsjPgwm$ zB4;X^c>P)LBfX;*OR8P^(_Hc1n1binI15t>Rn^Py( zfzgS8c=0+t+)KgHr|@T;6bpFQD~1lpzNKE&r6EYq^QCBbMu6mOYc9Hi0c*(0(b~?o z92v2&*1z$_>;?t5H^@?&5bBHpoFu5K3&jW$diikc+?~c={2-uo`x+KB+h}gsM`kBQAb>9vAS;SD+1bMdA_)3Q9 z{rYF5g|wWeqZ!95)7oFFjv;_Wxt!{(fOO2knabedbb$;H!0Ov1vp%0+Uw$zBRNYnw zI_=74>|NVIp(2EQcJbOJd}Vi+nRR(VvBML<5iBC(>0X49UGpHVD;jBA60;s?rQv z2~+D4u2$wPHwGi>>8(5F>8HBDnN`^sfD^DmO3U5_upwW)k%pPG*rgy)aJ*6Aw3+0y z%1=zK*`2PA@!jZnZHAb}O79K$9M$Bn1Km9GjL*X^0zBy+yYIGc(`{p=^56i5~H37vhFV;eSqMY+T+hsFAGV+a? zZ(eJNhE1=zb${b!=e8;Qx%9zq?#6DnahP#iMI0Vuu0!kaE{>O8peG(vN;n-|4U{-& z2O+H<0qpJ)55I-Uog9Y*B#7tf;a!-M25oz^)`xK@*~LA&!@d^Ycv_gL`7i`xwo>&S+b zfO)EKvda$CgMw0$`)-a)uhO~w)BVSLG6qC!-hQmmqGaKJA2-H80bM%&Fk_*84Oil9vlrHhLu_S6W? zT2x(Y%xm9D%<**1bA!dX?M%>8*rC`HS6BMKu=ZAAZLQtj_o^YKKq)O0FU4Bi9a5a) z?(QxLE}_Mp;_mM5)?&dexNC5C2=Hb-`&-Xi-S2*`Zy&t#XdWb2=Dah;E#o)F_z&*B zvqAxXBS_cyd3uKNey*!@+GauU%DYlOON~P zmHnMI=1Kh2k9V!<7a{yx`Mnwo$1$8Go`fo&Wbdc{lx^jQ@Oy-L89T z|Bf>EN(FbW2C>}|p}rXvNJNX3F)*^R@5rrtjhnh=6n^+sZ^kUFpqD)N#rg$AbWIij zx*kSuv^LSjX!r8ppRXKEFbo+b=q{U@tPHi=zSd>v@OaFclp!W zJvb9(VB=of=O_%JlYLru+#@X@lww#=z85Z17;(2Xk%oRH?1$4D<6in>aQamvZe$^ykeu zvaZ{!l{RHrsAcCHo$L05T@G@ohtS(;Kw{DZe*^<6%SNUDP~wz6p{%Dpu6j9tM}Ve; zEjMSeki7GofY^V@yGj!TZNy2CV~7|sJ0|wBkSzZ^il8$%u*b#K)!y~I`M8}Q<+Tr* zVc780%B~vx2Uc_I!oXtuC7YZT^*`B3o zb#!))RNZFQ&%}MGCHv=YIt#3pVf0LesMW}HDi|~JU^4%ahrfkpEwAa}{kf?isef1>#ws3<)OzEyWR|9}9NvGoXT3UeV^P_u2giqRf zre8>+wY%SDdtN_A#~*p@uRMTQnBkRG_cvbE+BpBM-Ln|Zr}>OpBgB>{;37%K7|U{2 zP*uCWx|`-3vZt7q^Z>N|H8C9;Y7x`TYr9D~8zL~3bqn1LkVQQFHf$vAHYsqhDmaF_ z1RD|5@w$Dv?lZ_nC?|D_E2(xWZ{mYf{I&7@x?JsIyohc}2pfG5yR`**JtvjD+9Umi zR{l?f<{q&Ji15H_i}PmBwB>GzA4ol{oXbFC=6|nr8X+We_rmV;w5$&hr+A2Igag8i zmMJl^F*>fQ?q>2aRmQZhV~V*>YdDqnXVVMISr90z zY*etHR_S-Prn{@#4>7QMo^oaW^COx(wzEgGQx_!LztnH+Gk3f&P($A*PGW+?+*y8^ zyhm8^141-PAwV!NuY?O$08=yh?MyC+GNxrFV9l*EZzjO-xJ<2CqoVZ;QPmsY{W}x? zr>S(jr_^q*P*Y}jd`f*VCn3#K`Ok;`=O+bG^a&Mn9g8(;rL~I7?Q21SuLW5p^JIJn zllmGgTZ~cLUuQj-={PhSb3>`Fx)$DVwJQNrVIo(kS9V7+I$??<)D}>jFd_gV0Ns__ zzad@ToCh+j&@_zC`R?+Xb<;-l%F@w(1n;+nij{yetm>keJ4xsEq&8Ky((>$AUEv-% z&SO>R?6fQOV>pZ>iVCbg1euq7xLeuGWc8azNqc5`+<`fu=3;w=q^%~G%xyJ{Uz}DP z#aU8fLC8LNYoFMNHod@uK&x6ux}vorisp))uD95{4j%+cEs6P(B)n9|lnmRrx@EH` z^c91IN-6kyWc98m4;Uw>xd;n|$}E9|wS`krE0ybb?4#}SyXu9y6M1y_Twwaz#)4o< z4lrA+7J{8ge>1-3eP}i3Cv{9UvuM*gxjq|AClZ{N!M!0tiDkMO-znNlxE%&b5p00W zwSMF8fs{i^=44aB=~}9bnaa+`EO!jj0m=TOTs+>bSTFitjI%hO&8FnLtbSL$UcEBd zTrSqdCnn+^8=eoi6acH?@3+(zcfyJ*cN-BWgWw{FKC~p=-$+T5HI^yoB+XGX@Y@nZqsJ9@?!cVouwatzKSRd6nv`dV(0C* z$k5t@%QK?<@LtOD>oE~#^>+Kfjo#VICHRWIsJ2f1qRe7b#A>CjYUO@*pOWkrK5;5C z^X&KkrFxI)Hx4z1m{bq_=7C>%V$t&0&*|kfMCSi+VDG6mtbphLgx61-bDIi z`IJw{MbmB$7cA^wj%CK1^vIYuwA0x*$#Ak+7D`RxWnR;`iAC(inlQt_zk1&}YB<-r za5L5UcJPKL(^M;cD~TsU?{N7jBSgn&E1R8zJKQT!jN~P3zg%;s(u(Y}>99Rzbs7vR z(zoxn?%71I*>wOyu72!htV-^m__N{pzgMR&3W!3Tlk&6xRN{f=aiM8V^mUSDx=FIz zE6N;Ac4U5DLH!%cnyh7?2>ZKUo9EAKIu_%^cE?caO0T1aBJpOlTs((t8CjMaBYq5* zo>ZyzedQJCDvI@J<&Qga+i8UHc35*bQDmet&@Gf2r3M>J2g|DCCP^?^K+M2oiH}3+ zJF2(h%~_?7YbO-?ZB`nWVl|f_1r^AUj(PYl~gJG_mw#@go@>wdimxJoPX2tIv)Zc>BV=Xg8CWC2V)O_k7|88tw+R z@Fv@g;?wO1+Vlt!SvP=|K1ExSjG2OmG7Z46xC|pD)%ywA*q*nh>lu;u1@sk-qQfhDTYJEn<2b>d5Pf(;6Z1*-bmPudy$dW=h{AD|Jt1HU z;C~b#dwj`ZIxwF#e6P)9NuYjMD(YjY16nIBq@96%U!>9P{+p)a}g45#YG=mH>H5YbY!^)P%z(}4%y zr_;i1NJc*?(~A7JKm6$#`h=3*BBh(u2QwO%p0h=&tsaWgNr^km9*jiH9XChcU)Eo< z$#TEi7Sro-i@WFZ<_!UOQc4^5&yTE~O;dP)M*ScJrjRPrO~^if^w%d#4n)p~42A?* zfqg{}Y-2jE>DXu6aqW>2x}hJ$&a$UtcBEWZ<*Q6vJ7CX~$C=ze4-SZ%0 zpEA{+lVwE5#=uXXj=6OS(Rw^+SgjJw97mLkbE^jCO1rnbk;LU#5jZX}bp-;kGD+x6 z4^BKhkd``X8fBTNY)K`(q(oGk=>6*cU~M00)bIarPNI4`3HfRSo2{rh7m?)D1g?wa zrk!MsbdH?H2eDs*&q0LxhIS&A{29jl910s! zmZdegL|69(!|98$>1x{D-qxR&0~4N9YP`l89`m-`;UGtWW+sgLaS**{EMf6mwd9II zb^}8#6r;U3+#LBkVJ~Qcn(gT^rF%{9LAH^+2K1PN$zO!8w}ds{-Da>Qk-!-A+>cOs z>#kphx7@N5WJ$)I&H%?hA=SF5>>LGi^?&X|McR1ob6Y4uR21st-<{d-vilVD#D3Yc z-`|Diz$U39MkM5T(9>U5ND`+C4;x_7D3e(svo~AOk~zNoZmuTuV4oDMQ<+@tw{)HY z~5_&}%a%OQae`aS{lL$xlJ}UCp(Za^g7>(iaWCJ=JEOH2pSXO?rEwZqX?v0LM-kzXpX{1iNj8NrM zmn>@nu@VIBI*5 zvg#ujmc4H@9>u`3EHY}swW)wgP{7)|6I2F<1u2Ug>QJH005h;qB4a6${1I5ndCD>kGl-%e-uIIU5Kc<^rpC>=J`gs(I zs@BxjS9x3!k1 zT9#{;?9L*iU^3_pR|q)hUa}&8*TY(aM zhT==T7M5~0pJ^1}`rvujiX4V?i*94xVZASb9GYH(Kzk{BMjRz2#dbL}G*y@(#yZ5@ z-P`1t@C%*klvaR*>yp7}%2eK)k(UrwmBZ3Vfkup!D`SplXD!hnk*!ZK64I{j&i}{_ zLt62^!L|Z0U4`@CtvM?H!QXaRW9^t$Q1f(qJ)j+0R6G2S^x;Jz{~_VK zq5w7T#(fD{X7F-nxg|z)jCs}~Iy9kDk5p$CO|7lKEV6u2cBpDyjsBg+*DXAjabs%Vi6D$W3uMtZP_%f3YTY(H*;w zp*6GQ#W{IHYS?yc^Vi~4%IZcj25(-?wbo-#el@idWE`oWhnlICl+=)VRY0wI$Q+|H zW?l$IY{h*au9Gy4%$k`Ib#%{jY_pqKsqX7S(X~E!&*y=$07l$ie=2xptq28(j z{Sv((>O;MpeEX+~8Q`T(XP()QexhDCFP$Aeg=8uXu6u8r`8y;Fa6v1ywhZ{DNo_Z# z3~k1se`Cnbo|s^ZecYGU^7A)pwb%07KVi2i&n7;a7Ilfn$%8aRS6nfoSC-ljrl8d_{kRtzJJ8eYgg->( zJHK_gG2@OFRJ6*l!&=T>b6FSp=90$!Di*H)YbQb03~+93L*)4)ZN8H_VoVgKtjX`n zwK7&El8emqdtEVlpWx2apEbp^)9)}w&!ZgwWg%d-_5699rey~@YsIVA#JLS`e&zY( z6?*B}ywtmnGEJD7}}(8%$26wE6)Yx|<*Dxa~k# z;?OO_StJyDQbW&L>VWmMr#s?%<8Z87ndK5AYrT?!ROLI?=t8)#f0qRp^1Wa#odq9B zHW04Yndj4|0R1)bNceLyB}lNDfGuByE-luuO#oEBi3mRb5j`DRw1FOLh|hdgRA5+ET+q{I<*@u0oqN5uma)pB_m-@T$$n#Ano!oZOGhW= z3;gr;bUndgj{JcwHdOXltffcV=MHMHZUb~2ZMlO!GKX#0%Kp$U+-B2N{Z;m^g%zai zSI=tFwEReTg;>PzDF9fhWj=A@f={);#w0DO6mc6il2qJ|Op%Z7619Fc{xIdMvgeLI zIYdFyw(<7AEhO=;ByT`AG>d4Z4Otf*^%^^erAB~_KN61Jku`2h@3-T0kfo2eE{bJJaLnzL5vP`A*ow?Ff)ArX)EA-^v{n@j| z=}V%>AQL!pDEZ7&bLDVHt$@SxL@-HwI-si(+qAE?3EHHKsi7Gx-bd@X;5sJHP9LE+ znx)QrVe%oFLpUqvXoGJXu!&l!Az;Lw$002?G$BQwl<{qfg!JM8uI?iJzP& zGWu2i>a)^5*8Gadv}6hEcig?$ui&NjoZVhns{p@BdW-=sFr?J`(QMkk2N;3K&p%B; zYz<-evsU4}0f$g*K-j4plUs{ief)NAE-(jvdlB(f<|p^%J~*&WW&6(p@Yi%xTrY^2U4}(j*NFQedl^ylQ<+l zYPkEV4E9GlBEN!vZKdga0hF_$!aEac>%{cdB-)KTjF;dGt^P8J;dwT+;V4b2Z8=)# zXF_7=c1&+web%Qef02^Oh9p%sjaN?Fm_XJ-Zm zD&=}Y$Ilk7C!YGJ%aUE*ZSJFnQH{h6#;L}SuSI?i1>fT3xi-sd{gAY=col$M1YjS2 z-`#cZ%(Z;w@nh^PZR56sRp!1TWsyfFI{$R9&cUhu1}80I2{MDcW$)wuq!2P32Ny4Q zx$FE&WI>#;pgHC|PAFP$>qp?Xk=JE^ewORe`nmdi&T*Wbb~7wgGlB75)JB~VhlC-l z!$@YDT?l~7gWNJ;czDxgLy+Y7+w;lW{SoxIea@7(}b5CrO%;^(g zQrD#wBPqI%b(vB&B#0_jsR8r_n@|z3OT(%(&t|TTIUAiAnhI+R$>`62wV*s))n8o0 zUCFQ7nXu>>VilJesH`-#T<=-Uyi`fKGBHlpB><&{MATt_*{vYo-isVUZo%>G)j?Nx zUYj~Cg$R*~%biK9IbHk_|LzEZKUZQW72<@#F)TT@NAOSr`a5QEPS?Fm=R+iKbYnCwkr0)c z#EXrtQ+iVR11Nlp4W++Ls%ztUFKTzx9=M!l@uwsZx~2?PlLPGxjZd^rtt)NFRy=l| zf%uT@mW%RtYV}bW;JQ*5n1|B86Egd_|BH~V@%%=}w8^dt~sJkcCrOgh3p6CrwbrCLJk;>JQx-#!vSH$HgYxGuNhkVrmny3?#1T=1 z{61vAJqI7tj@*sB;iVITpK+9Kh_~jUyZ#+L|L=DN&#+@kG$#j~O>*IArQ_|&^DBtn zwn-d{RDvc$Mhk39lT+ttq_6T^#}49lq|z6#L5A@n6X6?!l8V-I+PBTG1p_7h`jT~= zrMG(Y>U-$FVy_8|#x&zCmd6hMj;khu3io|Xq@NMBj=z~YCDOP+d(`#BD)GTR;tuL= zr!~BEOhdpMxaesYL-CD^d!H9c3vW!GUC2++3L>nikB4@2u#~~EW2qQ0q;5C~Qbs^z z<*?2?4FElh^JYMZI{<^cz;Vb&L~-YcI>};?qxgDEW84t_ z++TOGY=gmaSAw)>x2UiVu?U2xb8ig7HVka9f@5xh)^m&74~JQj)(<6FePNBbv%CT* zV-9NmBkRk_FzhwGV-+sZAlSgW2*3z@mek>Z+zYNaydjy7$PnG-8DX;i5it?+&){sJ zMzs=L6v{SAeKe*~{0R>_fNa;s{I!MPRy-QO{Q(omT(e zc0zLY&Lw5G)Q7Sx^B#CN0YD&U=d26vp5vqlbnggv z9`Ta)pPBznvLtn2A=-FO07}rF2OrB4oW;|r{TD)2uvC#Aw8!rsu;lvf!TfQvyYZ2% zPv!R66^7RR&sUV;2V#&Z%F{RDSPz?99(N$Z7j9qG?*n8^P8X-4<+gw5Y(zSF0OlWT zDUvZv2p&@?=kYppJkg5Xsr{Q-_ef@4vAKS^k$}#=jZzdJl!2uP*GyUyl!Ws6^XxAT2Wap+b?yekrbN z5sjE0@_ILVwfoS!$;X0RM@He@%<6G#( zd`uhR=MmB`rq|;0#8=cE<8&Xn6NXJy+Fib-s77pUcABiP>PLtMA8)`H-;3>5`w35NB4vjWx%PenA>MjX0h@)mXI| zAiFSoKCj}@2kU*MH470qBN!ZXJ|xQlS+>xs0N=>IuC$=gEsxl2(Qzx|im;mx(?&Jh zl0|ckIS?RMPTNeWq2YuH=>wk?v{aA_l>B0ZpWt-w4V$914nJ~ zec0t$l035KaH;V%nKx}~&!!HB3c)O0Iz&%3LTp~Sn;~&5&-nbI%MpAA>jfJ%FExpaRFHREzD0m6S2yNy=1GAxxYRK7C;8r&4a_mYJjV%_LVJHc0jdZ%5* z+Y6#pXI)??+DGq;)vbwjv9-}3oK_XLPqr$H`dNPb@@h&)<+#dCopv1$PyJ=(e;#bi z{z0YHCJCa+aQ@-^gz9=%ZdN$~rRB&mt(_(^;!sP%*Buf1S3El5y;Nb^nGVC@5SYl;IQ4J zqKoy=jpv3yZv!y<|471aEV+o*-x>@SjpKY9N92rV5N?(XQa2jwmYvnf1(V1PB#Lgj zlELUCVi(}IJi*h4R`4P>Yqm>cKPOddw1E4?iP#{L)?w-P_HLFV!j6|Kzk@JHD)-i> zZYR|upzb!|E7=5`*(eOFR zzU*zD*V%pC`rBvuvSoSm$6Jl?V2p&zv<`ono~cO>`3FgPLYm!QY~>D_Ab0FYraHGi zN4LWxkM@r#zebii9KScuzGi6tUC7%mEEitN&PYP>D^o9lGm_|+e-;ICt7zE@Zcd!;@cJJ zX?th;Y44l2UHsh~@>5YDZLV)NHv~7=-6%j-fSYvUurDq#h5HnZl}yEFqHNQLo_tKg z?#_*Y6~B?#C67|UO)Zx(_rq#lcD#>(n}k|1mP|*K=yVdvh8hk$D=8eaZ zcMKhG2QiY8C`tmJa;tysV_M`kxz~=Ainhz-K)gYW!KX2uO zlw&*0mb5OqKAJ555ZQ(tRdJ6A-W7}4OFmKu;Jq$>fwf9V*ANF5@@6Co{;b_K{##XF zDLdPZbGGUhi8P*A6yOS{j|_JDvjtVy*Gr~GITQ1LEYnjD{j*F@F2pqU zp)4UgB`QVli;Nv9AQ>(2?og4vOVivL`ZkYN!`JkX#REUdr;NGhb60|b<|jlp>Z^YF zm&Pi6bkx?FH~rb9ctQ^WgN5EGuYmQEfz*;5PrQUXDyfCh|4qa2eg08>r~O%XaOaU$e=MK7 z39(5fDH~|iz&_o`e#L!(YFieZcfYO-w&8&!3e$i|eYU4gpN})=CM$CIWq!ZGotmP}!x;lerwORhU+368Gx+U*@|2q1rc}K8%(e+B zNyl|drJ}ueay(3c!|{XV7F9|`HL(nUpmWtz?9btGooq|@P?y9Td=E}HvC&8~W!7oI9GyJskwz{>EX_R4^5DiiOBz4QZ36gHL^XQDii z>?3zp2dRzcdmiUcI#}t)ryWB%amz!TO5i{kM+_=-4(nn#+M}`1K{Y1ZUhTFGlEOrK zUAipyFvFYgSTKVlCoZ_H(&4Ompp+)6)XU%BdiPOH7VG3D-m zdiJ2(=ddah)TOVzR zfBcDUz^i<#M>>ldv~jy37aF^;Asn?{w2;JJ)J`)ILBkL`h-ux~7-OfRokpqBWfbbfU0v zztY?QuDfqb3#KTl&VwI-1NJ--ro~yG(mL-H8ZTgof;7!u;4K=36xb8*W`#%#P(Wkd z%Jy;`&F*Sc{LGF<-NSOr`8@Qk!Wk>~U5#MpLl#x6p^}m(k)oY!Q&Xy;vy1n5SW?nn z#gER%n^3;;A%lnAVy;RMt~-ad>D7|(sif_&#S=bM8&fj3O`CCx27SxJv5*fVaV`uV z>6kbw@b51avjC-RoTgiA2a~eab6Mgj76LqZ>shbf(g$a-s zV`Hy&*tZN*s7K#n)9+13^obcFcqa2!KBCxbaLrJOSUYMT)a3e2ul#L>ypvK z+EgKxy`z_3AzJBYaT+zv5|91Nw11k7Jy&&6Q#dYqQJ$-$=SCR*uyVRJB12JtYHx*d zir8u&_z8H`0YyIrz1+QM&4poi9Dv+``>lS$lh@N6_{0xHCsz{dqfMz@YHq8hXIXo_ z$=mg-n3t5gy>o7d{3?7k3Ft)L+W^;>_4l5nyeIg8^a5rS{KP%0n~lwHVgoo%kKsE1 zJl*=*b$Od?t#6KxR62%5`IS1w&{kDEMlISl#nlzMqeLiZ5$Qn%ig%=(5}v+q+rZ0n z;vT#rd5G?;Y&2_omT=zh;6eD16<+$DH9DJR(k^tM*+$8gs2Y_;B{=pFiKk4+a)CW= zB-m(ug;a~@)UBtTIdVQ645{k2EbK0vpbf;HVhRcnlukaUrj${)X z!BSMiHX1#ZMLa*)y4feKXRI!dWSYj2Q*~5Z&z)l*a>apy@50yfr*71t)Y6|)!tJTc zOU7}i4Rc>+17i%}1sgbXh^7ljrpTY-8vpg*q9cRIReKP#0gGyam;l!GP0bgw_9dJK zH;lF*fddUg@Pnsd8pFL<^$QlL%WD3Bn&VNj)ks@PUkIh1PW2Dt;y0C*iMpz`o5Pqm zppkt$jJCFjQ}Zmk2oRMar!%uZ_FdEo+=D5b^3voCkVTz7)2n5Q*nq!yO9`8NBuV_O zr+CA*TG74aPKU~;*%IY?5V9&%(pGeCKCZu%%E!h^^7e8uMX9me%0g`9$-~?9P>RAe zbkW7&T5++n(=J6HM7JR(=S(qO=QnNe;0w()NhcZL8D*(l`J%T3h2Pt>=`oGk96zK) zdN8%_?{#iMp8xAQH;DEBs?JTm;-7VHTl4=@Z5wh#gc2-=?j_iCUN-1eGGmlL{sP{< zvo}e5L}m57I<}?a&!#Q1G(yc&$8Ld+Tm)+u$I@}Bb*M2a2)}RTuYZV4kxTWtE_8HP z=LXIq&>EEiH6$XNsSG|&)RiEpSr8h#Nm%3V6Ok_i46!uK_2DmwVnGkSK}xNkfp*gOWGwWwNGj)O0`Otpnchew4PAu)-z)R~B=3lPvXIk1~ z_22uH?jxjL4LKfaqwXSo<$065G`#%V890eoau#`HlO#^Hpup9tc`7|q6L)7)5o3=w3BOVk@!%r@m)OY(OoW70-|0T4UKc zvx!5>WN6(d%7=h3!sauu$#6hN>Je6)QGSVZsWegQfFHk^WH1HkNIsc5( zqS2-Cz*Ga;wDR?Y4cF(RrX6p4kf^F1f7#}qWHa`} z0^k3+$6;29)6MX4vOPYTu(z?#P3I0JiA^zA8syk$$yEaF9*Kte7!&?^Xm!rz4Zm|| z7ZZ#y@PxCQETs>@dO_R#d?(O`3QouhlT6_vk=(AXe|I&a?aW9bI+spC!5*)G-u5*lK$3Y(!mcn!YQ&rgw8c9A^gA?Bs;pfJ8x)c9I zUUrA?Au^3DHp8?w{f~);roWE*D;Jzv6MY|-mpmb2@>^bnUPCt2KYT~sKV)Ql^v(!; zL5m_D`4J`mo5Eq&OJG4p;8xJZIhbk}m8!tByTi4BN+IT>$Oohx6s*svo zDIU#cTcr0yT3EQT@ol7H1V%O$NC?eqxJRV&=7vM9ZbzyTd$AD_A!n!QJjy#FtKN2I zkK8VgtFVSLF%zDYq_%%^y9&#B%#L89?2TV2aTv$OsHCbZvO$fP9L$%zE`loE%BJ_R z%?1(PxS?u#aZ32d*7HQo+;)UKUjRgYSIh^c>f)fv=ch0v`Z0WtR}An6u(u;Q;PLxC z3=ynIvhPqC%sl>j%CDKaM&0~%t^WmYx(AvNxb=MV+1EhzX^C)*B z+$JE(gr-hx{9wKk=Okzc#wPX#SN!2uN069!r1d>$vaM5XU{e7;elJ?cz>iTDZ7a$=ud5kzP6&kT#e&S=U}s5CuV< zjbmTibnPUHm=hG1=nX?01l7t8Y%820_L2fuTk$>(wr$2E2a1i3g1DD0l@TQdpRI_H z&VF`a)l@a7nK_ckqO*Qge;|x#t!iEgcpppWZo^8G=JD>%vz}=q@n!+fEXtMH|31B~ zKc8gtUhl&ueQgdEp$t*xmI*WHSfE~NWv)`A3SNb_L?%}bciH23rTP?!D?3p(tJ5w znJ5<|g;4O(dMifg=F1`Z_A96IkGXYzfxWIUs4X`d44)PcjtQnV?5K~*^KeRvNEuqk ztTQ?S$yU1z6>2ozc&A>`6R;~+6nCq? zn9UyBFZYOb!&m5Os18yZ(l4bG76t==Tqn!Z>V%iip(Z27RqJ?h9_bmeNl&c8wwXW} zipU!kt9@Qo0^dtN{`*o8;5Tm7OVGpbPG<{&v@0C@0<>)!Qf|fC$<{J(ZBNuxXS~g) zS}}!g7iF?PkPz7f_QtdfkQ8-FR0go@E}Fh4G+`hUpg$oQHeAyDJegFA zpG$u!RD963t+)8fr7*;Lmo5prKmS4T0(=BgavXE1&%g;c9+GDhbG4QaGJ?PSD;I!Z zt7$I56su5u@U0U=!iaZtrw(2Lp#L;gL({(U2!YCO;mP}BKfx%9L&mtN11J6q>$oZy8m_TP{G_gx_^fuD;0 zi_>i7GefmAADs}t$|Su{HOj&kESfZKGD$x(XuMgIgOu3Feq7dv8ZHFN%#1ke2oJ{x z?>n3maQ}Z=$D3RE*-SSy#G=5ilMWk28BL(_7k;Y%nBP?8$Qd+aAiVpn)_=NEc z$CAzN0rGCc;(ggqRSwf?>jQt5t7Z|YHtwH5|Ikqa*X!n7+NBlv)lmCoCZ(1r$~IG4 zew$9$J`azQrJjJuZUsA!?uAy$t-gHsoxXG_NyO|tusiixBkkbX$!2L?!Ds7T$0yP# zM^K*%f8!%^i+-2-th~z`jY+T2TL(hk7;rpCH!Ms_ZQC$bS#iFVyZ?}mtq!C3Tduu) z6Y3&~QMXt-A;n%@czRPvW?-+_ZJUb(4Ue(OY89S=w|>e)!EvCJhAf>ryC z7TryTf;Uln&R?pu{^Ng$rEU<_F}qNKGG>k%%bM~QCqCLP6_q8pey_;&atPTg&yv<4 zpC_xPHSLv}?RQrsUM*}+U)|FM(zTac;9b|7i9j>-k^=nJ6s_k=)rh8awh*Nu3IN&? zBS1;wg<@*UBo14?bZeU*#gH3oG($MCJ?&lhLsNQ!^bLhslvf59}lhz4cvoVgT`L zcO+Xb!TdOmHuFMg>Z1y61D^0+r$?xrPoYT2Y#}{ZyNy0(2Dt9udsVG?fg0Mr_Lbh$ zHTr)gfhXeWyfRDb(u*HGeQj3R^g1?7oLWh4vBbDu_rNR80z5=;P z?Qwr8r;jfBG~g{?x9-m=|FrwLs5aNn@@ZOV&Nx;qovs+#x_xVW^y-wxE zU+j7ZryW4AU03<5&H(vO;-x%)$aB=?t5+jkb1{q0 zko4Ww15Ad(jR=X27Elg`d{rsOtEGjWdICAIoxk-)ENxZ48%tr+o=*aj)L&YQRp*gT zoDGm(^y;v;-_z^bNPNJxG)FjK;>k^VLzkmuGiXm|Gt*U*Q=q7BDvh|74^zCf^`c^O=Nx_C?p03($(tcCqc7cze=hUL6`T=n=cKp+fls7bJV8 zk}Tx%zG;m7ju8O&e3JgS-Bf^RHhbO}2=IWDbFev=t+1Gl1-yFtll)*0*JUF&z3*bm zrCtNDIhL}L%JpjvZgs@$yK}TC?Xve}m^i{<-u=;Jw#e`%UHOoISde6(62aVZJ#*jb zBxScKJ4qTs2t3x%}>nngmAXa2b1=9NxxEuEw@uB*6y$vdq9$R9chdnGml zb5}%Ig)E_ZO+yI-F%Rr@fGbu0g%}HK;*vM)8tBBr(3P&NT*BLtwhs_P!du{QdurvTtrOmC? z6e0b=7OIm*L`p|Nnth2oj7{*+{Tp$&q|ysedEx__ZYz<%5yWQ9H#Z4Kc>tTLy$%RT9k4HW+S#!(6|**Ba;`b@{umpHA2U;SmV zxiLwGdgFoAYG6!qS>RQ_qN~sikKL$Ga^e{V3y{gJ0Sv_mz8n)J+3cE}! zR)fcj55LdUVgwQjr8lM5d>yHvoYMasmUv2bwA4!CrHRsN3RrE@(+Z8X#_ILE;vM!O z(}VDYsJ8)L;9nYbML*klgVP&7KcZGFwC^kHmT=-JdpzVFKl?5@s1*EZNMqCPV|9i; z{t{!@tgKEDnE?gO;{z&0)^r_O!1dj}Yy}C9ntsm7B9>wZ)s$3wu%!IOXQMyJ7r{tj z$d2|9|INHm3uo>rqaXvUDYBLE@rpm4mS0A~|5^Gi>HEt36kKr01j~1awkvz$_b|OX zO|+Y}K!Id|Y`ljxY}N#}cv&Z-xZ5&?dCB{VsQU{1J&1&EZ4@3^;34LS{;6Z3l?EJN za^1rWd?URuUaQ8;a6Vbmi*jkI-AAG7r$6u6`_2XGZ8Kd*%45KF$NY1!MBUoLay$)f zF2!i5TmZ3Hu3tq4^ZeJI8wC8auIUaYZpE=x|&Wv^Kl!Vm8 zg{Ke5f*xQ?%r55XmIv>D@_9&E!1iQ!r)5LU@))KW3LTt9xZ0qC2 z^FJy#u`lD^G}`*=e9330Lzd*nLkI^9)+ z*wZd4uq#r4%KH+B02MqSqdlk+e63}+0=qrVoN^MpDXzP(-NADZt`F4ANI3ihKTkZ>E@m_oO4HCsf=nOhuHgY= z$oJbGPzP>D>(I(8;fJq|kf3M#3rzu_pASCYwHh3r#$Uc#Yo2*)X-xkV^8M$nvA5A{bBJVg;_FrLgzE}Sg zWNV7F1P%Dv>wLYp&qlr>roTTdYv6mSznze`?`cd)Qtnh5Tm?W%r!QwQ{vs==S~A!k ztKH~mR!R3Ec}99Ka3)DnVl!5EL^Qe>&%+We{Qiz<>7Ew{`FJ2)&10_h=MSFS&dhen1Rx>Gi@cL%nQV3)$umT&org;KW5jV6$&tS# zgPxJr-rz>@m)$#bgW)%cw8N~#N|E@U=ow)FqX|eQTn~tCyPgY+gq_D3#AelV&ohsP z(-W$5(|@RgI}htryEQHcvufp`-P%7%g?SmaZirkv^_WGG5x{H^^H0_3=H`ashcZ{n zGB)G$FKg*Ri!mbmkb#`6;^irkw!VgMMJ4UOMtb@VL(7gZKL(yUcPu@~Tu;GU8IQ4! zU)sDzfN9!4F$^l{1w<5;u9#^wFA)J@MyW-vgY(bQ|B&9l2hg{xp=SXKhONut?^);%x}w zy|rf+)KmKDE{8NCi(2*?NL`H<%U4Ejd?^O$n+}FVqUJo_rlsm7O{d6&BTR5N1(n|V zsUb4{``UsP?ecGOPHFGvHcG@InZognn&yFo)~G*8Q1dFlwrF$n7Yr;`@x5W?jo>;z zfXzB$KXx{X|16ojd1`qE_zI04U_xk_G~QQ@5V!BKv0h^_1W{5 zZ*tbiSkkzzB~k4`OR0vK`|kvyBNm}w%I(v#b_9BX1C3C_nbPRE;uBjOi`ib-80ln< zaG2}_s!_TIeRHA!-}$X7UQbo0#lG`B{d=bv2{&j~{Vdl$xD{1&TwDwGFa1~{o(#hT zPxbd7$pcbRDt1Cx)w!9^_^zC2*xX<7K|C7h+YI0@U*8mcpa@-6a!&WOP`%8JDf#R- zG!5>|st;6Y{2t=sKbPkeaTv?EIb}o4-uPyl^lDJRl(DBm#j>l|%v5=Vdy4c^ORKkc zAMnF25mSl+;|xRQ&v_ET1bk^VRd|kx+ah0^RV{uI?!)v<@+h*^{pEayLgu2P!m{S! z+tFcqm+YOVmcb8UE><4$4x!{$yUGt4?X=V<_dlFC=eJH4vwhlCalumbcB~v$Q?7TA zH8*v4vATU(P-16Q&ah4N?P0sT#!PN^#iPiCZW@OPGw{9u^aAPnKE{DxFQ>*TncX_x z4NM5=WlDYe5r+^PX2B_Ltsp)(68K}nxU@1v7n#^Q)P!hdp%uJ8bsK#gfGUAo|Mpmf zl|2KViDEJ7(*b3v=RXtR=;A}ZJ>9Opx~RQ#2PFOYeF4Y5s%arb<36!Lg;SbKSQWVB z1&vYd7&Bic&Dp;<>H91?GEaWD=-c4kXBV%WZc)R8H>GUYheTLnc>&3qETJY#ov1Bq zmNG^e#U*(DwFAX7MbU2yG8rfLw%O=`!ZLKbdMf*QRY&dSMAyA+mUD2_;58O%hB1^8 zZ;Ut`UVfgR2%lw{)5~xy%|MO8wm%{cbCT(6UNh*uzwK7_4&pYHs_&Qj>`#e${9N)p z^G{S5wH($friI_j13p>;1{AUjzpKH++#Oo`xsf$(%ucDk2Nfo$;?IoQFIa!|L>-A{ zSGTOr{a6Pe)+I2Nf6?v%&pEiZASwvnkzd_g&UY_~sVFKz^d`XWi4KEcj0vW~Rb8b> zwm?T1;Y?3gUsX{FH0R)usI5H=IRJwvwFFQ3v;*nshUh9x#fp5y0OYPg;X5X9raAY< z1%qj=I^PhfFr1usrn&S&@m*_P31_DOdRo&dTPG8{Q-YLKhEjM$`z=wJeU$utapX57 z=&zJGObL7skX>~Z3bU|_MK92VQ|*OK&zky)QZCo1L~4#vc|NX1m`^{zvHaw1K4#Hj zDxb=6XiD_v8}@+jEuwFy??1>#M`*VS99ikkWL;?6qE!-9mso%wSTQ?W5C|C(c%=qg z(Yv04WQYABWEHl|v*Tao=Mo~^_!5@=Sc4MySCTl{?;}58ThLm>nwl9d4yG)o^lA<& z??}oMzCeqN1F1578;-Il8Au?)-cla;^aOWccAX-acD@dL5KOASBe;6o+886yE48|j zr9w7W7Q{M(4ZH+6v6&r+>!_22yKJ!y1@aVZIiaR8PJms52WQ`+2X<{rIMa*bO_1jf z<|e&*S|&Din&fu=h@JnYs>lt~!Y#S7ux&nyMK~eict+oV(GFMWv?VJ4q{Zw9rPb+J z-M}Vd{@@M+dj?Ald>tXsXe_!-O2(}RrAjV?KOj*<8igAQ{k>c{Rh~?N-+(Od1Y?*9q0P`*-%W(tK^64*{zdOwA@oPMGi7PAkkRQ z9W~UeV0VP7Acm!ha8pPKE2C+y-Cp_ksH@rLZXvV-*rZJ<#0Nzc5sKYH^IfpywP|_E z1G_QiZlp`@c%MSrQ%r3w6$?9$lnjK@(UC7y8lp{{?Mc_G0Hk=E9&AZZ1Y<41?LL)J zX|UF6X(U3|HZdrl3NV$9EEdpL<&DtI$DLuLN6WG;eF^moX5E$@l(6Zqp2>a+nV#;v zUj4lKYPuP5UEpOob-pke{CU9|bM*>HH1vtAO+)?^QvOgQ_=7Q*2hmpe%Y~4kUK!e%PFvBmut*zS%`(#vim+Q{;XBb_%oHUOaXgC`-o!R(*5|Ph z@~7qnZ}{?`)Vc8Sl3j;E<$?NK8?S!oJ{1>;*J!2S^R{vGvLugc?O$=_Dkj&z@lRK; zPSK-)w$?DW#0j~eISDlANhBrH%~{gSqT>Z;I_L&?+EegG1`q7fZT);XYaXxra@9ad^4MN_{fyA(;NSqfa$38P=t%YzPT1c;KgL{`NY#$tb5LtJBQJs6vR zb@;-R9}zMH$zHCF?Sxf2vv{Qj-EKjheMh^QUY`IIHl} z&@OH!XRd;~VU!5Jbt3GSx^5d539RcdGSm)~H_sS$2;=@%)Kp9&E@KX4%@!5!pn0r?WQ9(M7NYOstib>NEz15-;bQTLi)(pz^zm#lR4a~ zm$C7l3HY0S_%j$%J$^e}cwux=i%4I9J!)~38=iwOh#X1JzW3;u{^(<}9nkyDgjM+md`w~EbmqXrnR_+5v z>aeOFbH(7hrfC1yueI`CcO?QYZJV&`AMiH>etE$gOJ%z?q<`|!(qwU9s_F%gC>*Va zC;27J^nXvoRzS;j)Uk02u?H&IIVY(i(dH%i{70vF6}Q%3u^6qKXo^q$2r;U>sF1Ve z*997x@V?9Q>vCY~-oD7j!S=H7#NOa=e|VF~`!m*JSfRBD?u+VPAwcWpH`f}2=`3f8;mDsd>gKc_uE7vzu%n(D~ahU)Xr z4s3%Kvr_tGzt6jaCr0Qv%yzgaOdjN*JCzvpqq~=;-zTgY!)2Vu#azwa+U}6?h>`t{ za9;AHoUFHt4rLP+DX*&SqtefccTL3Z)6Sl%7(YB-5mzmsXcBwNp?+$EvYAOxlU1H0 zND1VJp*6 zlX1BRF%tmU6^g~ot^)znUwAv@`Gw3dALroPE>ATwgUzoh#IBFF6d4q5iXNP{)JTA~p7&vh z{eWNNIOh$(=R4%di3p#P?EjmCeR1y1hT0SKa;tNkQ(k{qO#Yg@Ms0k2?#n!NH9Nwj zW0$MCx1JXQc|Hg1U!M2ORu|Y68}G>d1u6cOGR)4uI=3UGhs+3*(h>2rJiSS%+|#(% zxaU!ouP;P8A-3J>h0*Z&l=9WgPI{71vfty`go5;d5q9P%FEN6A zx6#-SE$$roNQMs_RbvQjlv4Lf4Ih7m?ICId2~nJ%f!@ugZOiJY=q34M^m=DNcTF>a zZ0Vi{(eS<_3F^gyqynAiet6wkr}4Y26>X~bYXS{B&ch%syf;noz#^U^uv z7Gp0QiYYZgXJY8vzzBJ53uUXJBt?v^%tS!m}ZMrgU*Hg-R8kHLRO<0Lcw3E z46};dn%oJJ$i(x~a~@jsElaMSET&%Aj@zp4N-O^tzQKV&qFgTYZlZNjXB4-<}nXp;+tbwb-{6CbX&A z_vw~%i~dpXKgw{L3=-l%Y9XF9ds3U8agT_ofE$qPMcmJ)_V|$J-}vjr_F6%?l32G9 z%J41NCWLed!%bxkv2Nj4q6ugIg4rIHZdJ8^(JzI)q%hejItY(KbJ*7MyDMl^vVUQ` z`Ce{8quc}>yYyMnQ_g*((r0OjrBfA-E%8@#RCK!Jdi-z2H$sK3=adMntZ%Q@bgvZg zS*TG?hz68(Sq)&xgj z%P^fGRwBgFW9#4h6;j8$igd3+ufX}l22In`ScT)jYA7h{byE^1W?MkZifWIeN&t8K zscXS}$&3J&UiUTQ7_B^(RmQ}g}f8&+eXs_Mws_5Bl^JA3!E<{{kA z8;qp{s)Dht>IW(#yJ$r^D*L!YycQMg=P!j6>M9yH&f-7uoLr1(W9@l8mqQ+iw0{=0 zkGR4jLECh`aVlz=2Cya?yqn`9o8-jWk$n?NxkNTUA9UBn4HAD0Td^LkRgKe25 zS7p@+0Hm%kbD7Jv%y%w$_-m5%hmr!5BsTis356E52&wMM!o!oawH0y7MGFUzs)Kr| zM!~v(I&6tyTFtx$(R}Hg(3@(WHI3h&zaKrlt2`urIDSMXb!~ac8?nN)W>tiuMl@*y z7rGcKJZRIKJbSxT|545*UXGqQ#kZI{d!THlitW&?!F|s#YL~gn{hCi)WLaYPyz$sD z>e$ZW`R=B}jHG|qrdTkN?+7i&%%?q0%Jsc@oBZ>}^+}r&kz}jX&L4!?(VVCJzA7u+&C!*zzmm)*5O!$V8wD*5REyey{tK~6|7T() z#J_6!83t@-#NV9CdhRX4pA;zgYDHrIcq+)GKcN65R~-(xsz}!ud9-!08sYkpH4*9= z%gRg3r&e7Q(f*)5k~kF}D5F(bCRn+rLXlAEXD7yCs1HDbiECX42okgB3xX2S^nh6r zM0RmsDxoVqyR>TOisY&dI5POoH}*R2xkf44vPeioK&f}HKTs!9xW*|su6vY(j^n2; zcNN>zIb7ZMl&(}11#eBhnYwphbL1-AiTz}ag$r9q#$Z%x$5HYqqu!lm371l|ORf)u zgr#}%+!u*W>gp1e!e!IgRts?;Kl$?Sijc%-kWF^s%7}avYu&uQHAK;V1+7IJDs_|4 zC@+3KtF7HgMgLY$yq)n#LwD*O=6Y``3 zeIA0tOs%oAA)Fbl7Axr*ltuJ&nh6O!V93dZ7N$#uHecLTDp9*1D7(&)Dn7v028s^Lo zbWoQz%)0KZb=vQM61>@;QmDG09i$fL_x}b^KXB@G0y2AoLiD`GW3+5lh$`j+w3@t` zHylBvCDhs^&CSPBi!#-Ep~D4+6;*VjqSsHlnJ_?_?7mjMfJ8gN2ehg7FE_S zaQo}OdK~K-tiCpWk+v-Ro$&`-ME{vcKXMD1g_rqrT{3nPP7|&cU3VX}(zT~;2ay% zVQb=gBPre=|Mu0TVO?$47KAd-2@9+nfQX-eTSOdrcC!-0N2OYFw4Z8yZOQ8`g34Y&rVQZtE%W%AjFn0QbTe6Xn3@&*dlWP&{YbNa$_;Ei{dVw zr_b6KmL*N-3ZmDcx%GFea=TQdq83@u3q$u9!#2ND-p}=@+65MGZ=P*Tr_)x*<)DfN ze*g+y&S%oIDC*UB>^+m{3xmhJ(_Q?$A~3giX@3RisS@5I`F zHX9T3HQ?q^0AzB_Z%(fTKM(gO|MwA z1iWS3&ab4)KdKwe1>V&4siiOEW%H_EM zLmQ;VoiAA29n>xMzUDhmT%2(^JZzjvJxB5u3N@#c!nURD{_z!4p;R;#)` zMd_;$I}y+EjXsT0bq(ss?zHdZe&}c3)|VQ#G;(VdLNpfux75(Kr=8>=S~tQ1YX^X* zo3lBQ_c!0_tq5K_z)0|psDqsd-4Qo%+D~GbjUp*ISwFL-42Vvqv}Y@^4r9v5(KjT< z^q99z`DeDYVxF1846IE8kqa4S*8lF(tuIN)?pN1>N%hToP%lqmFK-AEjHe6(;TWdB z+)XTcOd`!jOeWh)SLDX&^Y+?gc7e$azx12ki`P7oIj`=yYn~Z3*Uw35!h}edA0~s? zK6h`SH1&9 zZl^dLx3;vxf-KZ)Y9=0rl$!J)qp1oXS}6u4(Wm;jnCa!rk#cVMbDmkeM}~iZ8gK6Z z8`N~cdpQtz#&$|j=au>^4yS+lTj^kr1n!*Y`-AM+i7$ICnL>d*M%c)f@b)UHCM_(Y6{KWb(rHf%* z(2-@xYQPW{%cG>QW<9hJWHsEljC6b6OkOs7!Fp?T=$Q`D9=>rt7W_gT9m2Vw+oiy@k#YdD*tR#HM>N3JopcQy_tAu0K ztuzU}1+_t8+Y9~xPNngqE^7eiWuIHf5CnEG*6dY8FF;P6LE4f^EKxMWo4{w=R8&2q zL~Pov!JIz(pM*t{WlTROb@@GcO>8wtSTwmINasfIXza1z1eQr#&RFo5ZopJ=^Qp4x z``NE^rwKC*r);8o2%^&WfktaW?w2`0#_Evb5jo~nC1-GKV_L@wto{VaKO-r?v_rxn zqnmW^mX_@!^;I{){0~$EGR(yBPKrBaiZAn3sV`jWCN0!I;B2XKfDgWZ1^X(pyXyN+y;z6JHX3!|* zNJtubVMz!`(ceqI_dFE&*~@sYaL*9nBj+K=EPUw!NxoZhtZr!0NlV{S|7}f>7=9yl zk&PkAIWT(92^+Vb6Kv;(6OB9>{N!ga^tHu3lyTpkDy+hrbG6?k{(fYz@kZ+LsTn&Q z8|0+DX-M7}t7MlVt+1VxmW_kFOs?U6erCB8D? ztTNQ#iJRoEpH2}cZmWw8M_<~g4P11yxb3C+wytoc^~8X>!+6eUM5m^bs3U$}(sD-s z1|V_>021ueWMw54i@*^VyKKcN^AdxDwiNaJ|umHL>j$>4XF>a1$t%^=qe6irsHwQg~qKS}deiR#@|Xyy79Ax+&%_mo{>` z|5+8l%U^-`r(fFog$XY&*ihn|3W8R|9F@e|e}aY)e?HqwfY&$w3sjtuJ%etfN7M}+ZvrDjg_%3-10zSs{0H|AH!$9KDPhNbN7KcZU=7r zSe>Blo$n`^tO)Cd6bQytV_Gpg%#9a#Yz)zOe{pDhubPzfcPybJ)GSNCCy7^~&&&St z6^Gj}>EP!Ui%DlJ2}`CSbh{0gV{1#kbMnUI-e!|QZ^cH2x(5cp$8HNK?YlSC4w86+ z(c43Yh5dpV6Dvg>LPIg``4UjBt7bIyZij7fl=3cM^KIn$nDrPH@}?t?|#Ctzzj@R8#>amZyu63xG()N(w$Kq$M|YQ)o) zq9z8zDYF>ZVcNSO|CwZ^vdM+m*D_9ZdTG{El{qkDB1uZ<=#BY!{%p@-2ARbvsle4F zZilsc?pslT@WjqGV<&uvg(sW*o?LYq)Yu;VHdM4DG1QEvM;yA*?Y8_L^KVih2N{K` zDd?c*zs#G`%!Ss^Z~W23gCqqHLx6kR@7Gz?hi#(f0AcI~z2#aYWK!=ApUT*!Uo4Jj z&mOGT#<`C6C)5wxoH(HGAH*btml8-fCgItmTg)4+nHwbMOqW!AD9pOt3nb6ubT@_$Q+U9NIOcFy1Yt9 z(gj7FFUecK=@jN|YKkUs&)}8Y=G|%EhrsVmkb+OD)cf$SPQklNh5sKc!I48K_N@#Q zXmmbz;_;E-rk@V)W(==n;Md`2ZhS5|s98ldmQ~0-;mr;!LBZmCmCCzo%H`T9!gr zm2I{lFW-8>Fu-M#)|kzsv%bomY7d4=`HkijURb2cibM1W^g0^#9zGH8Ubqk+g_74? zzi}zl@HlYEyU4QJ2s5*#P42*S+D8grlhrw6>#5b6-!X36jsgg15-PGrf$S6z0_HlD zk>!-Yq8)~XJ9OxR5Ua#4>U5F_SEd9gk5?xY3|e^Q6HsY%cp~`43*9nhlyn9xW+C9} z36QwYXD?nl&yyE~`AHqhaDfj; zKA#~Ny!Ds3;`H@RZ;vZJZ1D6u1P&QI3U}uNk6xS=TSFM*mk8} zgP4nh39j*I?Eo8JuTDGq3Lbn>XpLFm$G0}=JUD|gfb6fU)hc$W_S*z!v>tEkFOyaC6 zKe(SSJ`|xca;mji_UC|oS1spo#A}KNx!#d*==HkXx)+u-hWp<$!~-1Chfpo%p|!nL z4`x1MxU2P@w8UMoFkZ)|lCI~V+F~zOLV&HW^6C#uaaZt%r)t)DcEaP$-g#LX8k!m&8vyAN_FT?iMy7oW{Fjq4AfB`{X` z_V==}0gLGV*jOznEv?p&akDtu?*PokbcSQPyO!KZjhq)v5Py9>LIVfXQUNuP17v>7 zy(zw6Fi{RN``7yFe0Ve)^xAX7ENx6P7_C%^CTVQ2`;VPxcX=;~RW##bxY8_&ut&4#d!9Z)nAKSB-!s(~(eruFr8vg+v$d5A zQ`Bm~vV6Ra6s#(T^WXFyQNW5*%LDV#?lR&wRV#1sQ_Nyyu9n!KcvDUVll8KbHrI)JY~P8X3a44uLUbT=0SLw zez%F0z`SOSzqLFgcQWX69tkiIMGRR>hQWMQ?T=FKM1?7=7Z1$1G4HF$1)K@2>en$S|<@#QB4p)slN3Uhqnd(~DaTU$#^d?=G)DBrC+QLB|Rndpc?s=f7>0 z{_Y=MV|ZZ6{G`}y3CoNdd*s5t*hKXo)J~--yOSE34HLDnMis=(X6;vv*jWE}Mo9(7 YJqg1R=HZ7aAR&^JnEbnfH+nw*2fdP;*8l(j literal 0 HcmV?d00001 diff --git a/planning/planning_debug_tools/package.xml b/planning/planning_debug_tools/package.xml index deadcd54fffdd..14db1277cc02a 100644 --- a/planning/planning_debug_tools/package.xml +++ b/planning/planning_debug_tools/package.xml @@ -6,6 +6,7 @@ The planning_debug_tools package Takamasa Horibe Taiki Tanaka + Takayuki Murooka Apache License 2.0 Takamasa Horibe diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py new file mode 100755 index 0000000000000..3b80ce4e5588f --- /dev/null +++ b/planning/planning_debug_tools/scripts/processing_time_checker.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +import argparse +import os +import sys + +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float64Stamped + + +class ProcessingTimeSubscriber(Node): + def __init__(self, max_display_time=150, display_frequency=5.0): + super().__init__("processing_time_subscriber") + self.data_map = {} # {topic_name: data_value} + self.max_display_time = max_display_time + self.get_topic_list() + + # Timer to print data at given frequency + self.create_timer(1.0 / display_frequency, self.print_data) + + def get_topic_list(self): + # Get list of existing topics in the system + topic_list = self.get_topic_names_and_types() + + # Subscribe to topics with 'processing_time_ms' suffix + for topic, types in topic_list: + if topic.endswith("processing_time_ms"): + self.create_subscription( + Float64Stamped, topic, lambda msg, topic=topic: self.callback(msg, topic), 1 + ) + self.get_logger().info(f"Subscribed to {topic} | Type: {types}") + + def callback(self, msg, topic): + self.data_map[topic] = msg.data + + def print_data(self): + # Clear terminal + os.system("cls" if os.name == "nt" else "clear") + + if not self.data_map: + print("No topics available.") + return + + # Get the maximum topic name length for alignment + max_topic_length = max(map(len, self.data_map.keys())) + + # Generate the header based on the max_display_time + header_str = "topics".ljust(max_topic_length) + ":" + for i in range(0, self.max_display_time + 1, 20): + header_str += f" {i}ms".ljust(20) + + # Print the header + print(header_str) + print("-" * len(header_str)) + + # Print each topic's data + for topic, data in self.data_map.items(): + # Round the data to the third decimal place for display + data_rounded = round(data, 3) + # Calculate the number of bars to be displayed (clamped to max_display_time) + num_bars = int(min(data, self.max_display_time - 1)) + 1 + print(f"{topic.ljust(max_topic_length)}: {'|' * num_bars} ({data_rounded}ms)") + + +def main(args=None): + # Get the command line arguments before argparse processes them + cmd_args = sys.argv[1:] + + parser = argparse.ArgumentParser(description="Processing Time Subscriber Parameters") + parser.add_argument( + "-m", "--max_display_time", type=int, default=150, help="Maximum display time in ms" + ) + parser.add_argument( + "-f", "--display_frequency", type=float, default=5.0, help="Display frequency in Hz" + ) + args = parser.parse_args() + + rclpy.init(args=cmd_args) # Use the original command line arguments here + subscriber = ProcessingTimeSubscriber( + max_display_time=args.max_display_time, display_frequency=args.display_frequency + ) + rclpy.spin(subscriber) + subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() From fdbe64a470eca3c016b69501ed7a76bcd574a53e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 23 Oct 2023 17:28:47 +0900 Subject: [PATCH 150/206] refactor(start_planner): guard for invalid lane id (#5376) Signed-off-by: kosuke55 --- .../src/scene_module/start_planner/start_planner_module.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 6b56c88eb942a..62f3dbfe191ac 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -639,9 +639,7 @@ lanelet::ConstLanelets StartPlannerModule::getPathRoadLanes(const PathWithLaneId lanelet::ConstLanelets path_lanes; path_lanes.reserve(lane_ids.size()); for (const auto & id : lane_ids) { - if (id != lanelet::InvalId) { - path_lanes.push_back(lanelet_layer.get(id)); - } + path_lanes.push_back(lanelet_layer.get(id)); } return path_lanes; From 69813cbc879b7cac763ffd7415a346ec2a88441d Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Mon, 23 Oct 2023 20:27:52 +0900 Subject: [PATCH 151/206] feat(duplicated_node_checker): add duplicated node names to msg (#5382) * add duplicated node names to msg Signed-off-by: kyoichi-sugahara * align with launcher repository Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../config/duplicated_node_checker.param.yaml | 1 + .../duplicated_node_checker_core.hpp | 1 + .../src/duplicated_node_checker_core.cpp | 9 ++++++++- .../system_error_monitor.planning_simulation.param.yaml | 1 + 4 files changed, 11 insertions(+), 1 deletion(-) diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml index 5b8c019de5a20..54b4f691b6eb1 100644 --- a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml +++ b/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: update_rate: 10.0 + add_duplicated_node_names_to_msg: false # if true, duplicated node names are added to msg diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp index ec086058e9041..9d806754f3c20 100644 --- a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp +++ b/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -48,6 +48,7 @@ class DuplicatedNodeChecker : public rclcpp::Node diagnostic_updater::Updater updater_{this}; rclcpp::TimerBase::SharedPtr timer_; + bool add_duplicated_node_names_to_msg_; }; } // namespace duplicated_node_checker diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp index c9aa483724532..46c02d9d6e133 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -28,6 +28,7 @@ DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_op : Node("duplicated_node_checker", node_options) { double update_rate = declare_parameter("update_rate"); + add_duplicated_node_names_to_msg_ = declare_parameter("add_duplicated_node_names_to_msg"); updater_.setHardwareID("duplicated_node_checker"); updater_.add("duplicated_node_checker", this, &DuplicatedNodeChecker::produceDiagnostics); @@ -63,8 +64,14 @@ void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticSta std::string msg; int level; if (identical_names.size() > 0) { - msg = "Error"; level = DiagnosticStatus::ERROR; + msg = "Error: Duplicated nodes detected"; + if (add_duplicated_node_names_to_msg_) { + std::set unique_identical_names(identical_names.begin(), identical_names.end()); + for (const auto & name : unique_identical_names) { + msg += " " + name; + } + } for (auto name : identical_names) { stat.add("Duplicated Node Name", name); } 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 c396e2e9f5ed8..9784259490ec2 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 @@ -40,6 +40,7 @@ /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } /autoware/system/duplicated_node_checker: default # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } + # /autoware/system/duplicated_node_checker: default /autoware/vehicle/node_alive_monitoring: default From 20c0d15a60f908d9028bcadf1d25cafa920b4885 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 24 Oct 2023 00:52:53 +0900 Subject: [PATCH 152/206] fix(goal_planner): fixed goal memory leak (#5381) Signed-off-by: kosuke55 --- .../scene_module/goal_planner/goal_planner_module.hpp | 6 ------ .../src/scene_module/goal_planner/goal_planner_module.cpp | 4 +++- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 251b72ea6eadb..a072c77526ef5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -166,12 +166,6 @@ class PullOverStatus DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER(std::optional, closest_start_pose) - void push_goal_candidate(const GoalCandidate & goal_candidate) - { - std::lock_guard lock(mutex_); - goal_candidates_.push_back(goal_candidate); - } - private: std::shared_ptr pull_over_path_{nullptr}; std::shared_ptr lane_parking_pull_over_path_{nullptr}; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 6253097b1692d..6c431ce5806c3 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -560,7 +560,9 @@ void GoalPlannerModule::generateGoalCandidates() GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; goal_candidate.distance_from_original_goal = 0.0; - status_.push_goal_candidate(goal_candidate); + GoalCandidates goal_candidates{}; + goal_candidates.push_back(goal_candidate); + status_.set_goal_candidates(goal_candidates); } } From 3d2a7f6038627ca52a4f0c2af6f5a8d56bc6962b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 24 Oct 2023 00:54:47 +0900 Subject: [PATCH 153/206] refactor(goal_planner): rebuild state transition (#5371) * refactor(goal_planner): rebuild state transition Signed-off-by: kosuke55 * fix occ Signed-off-by: kosuke55 * fix canTransitIdleToRunningState Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 19 +++--- .../goal_planner/goal_planner_module.cpp | 59 ++++++++----------- 2 files changed, 32 insertions(+), 46 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index a072c77526ef5..34ffb61dcd8d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -240,31 +240,28 @@ class GoalPlannerModule : public SceneModuleInterface } } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - void processOnEntry() override; + bool isExecutionRequested() const override; + bool isExecutionReady() const override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } - // not used, but need to override - CandidateOutput planCandidate() const override { return CandidateOutput{}; }; - private: + // The start_planner activates when it receives a new route, + // so there is no need to terminate the goal planner. + // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 6c431ce5806c3..e71b1ac7936be 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -124,6 +124,13 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } + status_.reset(); } @@ -228,16 +235,18 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput GoalPlannerModule::run() +void GoalPlannerModule::updateData() { - current_state_ = ModuleStatus::RUNNING; - updateOccupancyGrid(); - - if (!isActivated()) { - return planWaitingApproval(); + // Initialize Occupancy Grid Map + // This operation requires waiting for `planner_data_`, hence it is executed here instead of in + // the constructor. Ideally, this operation should only need to be performed once. + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { + initializeOccupancyGridMap(); } - return plan(); + updateOccupancyGrid(); } void GoalPlannerModule::initializeOccupancyGridMap() @@ -264,22 +273,6 @@ void GoalPlannerModule::initializeSafetyCheckParameters() objects_filtering_params_, parameters_); } -void GoalPlannerModule::processOnEntry() -{ - // Initialize occupancy grid map - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - initializeOccupancyGridMap(); - } - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); - } -} - void GoalPlannerModule::processOnExit() { resetPathCandidate(); @@ -446,13 +439,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus GoalPlannerModule::updateState() -{ - // start_planner module will be run when setting new goal, so not need finishing pull_over module. - // Finishing it causes wrong lane_following path generation. - return current_state_; -} - bool GoalPlannerModule::planFreespacePath() { goal_searcher_->setPlannerData(planner_data_); @@ -907,6 +893,12 @@ void GoalPlannerModule::decideVelocity() status_.set_has_decided_velocity(true); } +CandidateOutput GoalPlannerModule::planCandidate() const +{ + return CandidateOutput( + status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); +} + BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { constexpr double path_update_duration = 1.0; @@ -945,7 +937,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible @@ -998,10 +990,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = - status_.get_is_safe_static_objects() - ? std::make_shared(status_.get_pull_over_path()->getFullPath()) - : out.path; + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); From 0938f5b69b4046216e05553a2cf1b8a966afd65f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 24 Oct 2023 02:17:03 +0900 Subject: [PATCH 154/206] fix(goal_planner): use recursive mutex instead of transaction (#5379) Revert "use transaction instead of recursive_mutex" This reverts commit 654fa8cd0dbf036cf693abbd137acae9936c327c. --- .../goal_planner/goal_planner_module.hpp | 57 ++++++------------- .../goal_planner/goal_planner_module.cpp | 49 +++++++--------- 2 files changed, 36 insertions(+), 70 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 34ffb61dcd8d5..21372ed6e2c2f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -76,47 +76,29 @@ enum class PathType { FREESPACE, }; -class PullOverStatus; // Forward declaration for Transaction -// class that locks the PullOverStatus when multiple values are being updated from -// an external source. -class Transaction -{ -public: - explicit Transaction(PullOverStatus & status); - ~Transaction(); - -private: - PullOverStatus & status_; -}; - -#define DEFINE_SETTER_GETTER(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - if (!is_in_transaction_) { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } else { \ - NAME##_ = value; \ - } \ - } \ - \ - TYPE get_##NAME() const \ - { \ - if (!is_in_transaction_) { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } \ - return NAME##_; \ +#define DEFINE_SETTER_GETTER(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } \ + \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ } class PullOverStatus { public: + explicit PullOverStatus(std::recursive_mutex & mutex) : mutex_(mutex) {} + // Reset all data members to their initial states void reset() { - const std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_); pull_over_path_ = nullptr; lane_parking_pull_over_path_ = nullptr; current_path_idx_ = 0; @@ -136,9 +118,6 @@ class PullOverStatus is_ready_ = false; } - // lock all data members - Transaction startTransaction() { return Transaction(*this); } - DEFINE_SETTER_GETTER(std::shared_ptr, pull_over_path) DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) DEFINE_SETTER_GETTER(size_t, current_path_idx) @@ -200,9 +179,7 @@ class PullOverStatus std::vector pull_over_path_candidates_; std::optional closest_start_pose_; - friend class Transaction; - mutable std::mutex mutex_; - bool is_in_transaction_{false}; + std::recursive_mutex & mutex_; }; #undef DEFINE_SETTER_GETTER @@ -291,7 +268,7 @@ class GoalPlannerModule : public SceneModuleInterface tier4_autoware_utils::LinearRing2d vehicle_footprint_; - std::mutex mutex_; + std::recursive_mutex mutex_; PullOverStatus status_; // approximate distance from the start point to the end point of pull_over. diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index e71b1ac7936be..95ac47494d016 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -50,25 +50,14 @@ using tier4_autoware_utils::inverseTransformPose; namespace behavior_path_planner { -Transaction::Transaction(PullOverStatus & status) : status_(status) -{ - status_.mutex_.lock(); - status_.is_in_transaction_ = true; -} - -Transaction::~Transaction() -{ - status_.mutex_.unlock(); - status_.is_in_transaction_ = false; -} - GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::unordered_map > & rtc_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + status_{mutex_} { LaneDepartureChecker lane_departure_checker{}; lane_departure_checker.setVehicleInfo(vehicle_info_); @@ -205,9 +194,11 @@ void GoalPlannerModule::onTimer() } // set member variables - const auto transaction = status_.startTransaction(); - status_.set_pull_over_path_candidates(path_candidates); - status_.set_closest_start_pose(closest_start_pose); + { + const std::lock_guard lock(mutex_); + status_.set_pull_over_path_candidates(path_candidates); + status_.set_closest_start_pose(closest_start_pose); + } } void GoalPlannerModule::onFreespaceParkingTimer() @@ -447,7 +438,7 @@ bool GoalPlannerModule::planFreespacePath() status_.set_goal_candidates(goal_candidates); { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); debug_data_.freespace_planner.is_planning = true; } @@ -455,7 +446,7 @@ bool GoalPlannerModule::planFreespacePath() for (size_t i = 0; i < goal_candidates.size(); i++) { const auto goal_candidate = goal_candidates.at(i); { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.current_goal_idx = i; } @@ -470,20 +461,19 @@ bool GoalPlannerModule::planFreespacePath() } { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_pull_over_path(std::make_shared(*freespace_path)); status_.set_current_path_idx(0); status_.set_is_safe_static_objects(true); status_.set_modified_goal_pose(goal_candidate); status_.set_last_path_update_time(std::make_shared(clock_->now())); - const std::lock_guard lock(mutex_); debug_data_.freespace_planner.is_planning = false; } return true; } - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); debug_data_.freespace_planner.is_planning = false; return false; } @@ -513,7 +503,7 @@ void GoalPlannerModule::returnToLaneParking() } { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_is_safe_static_objects(true); status_.set_has_decided_path(false); status_.set_pull_over_path(status_.get_lane_parking_pull_over_path()); @@ -609,7 +599,7 @@ void GoalPlannerModule::selectSafePullOverPath() std::vector pull_over_path_candidates{}; GoalCandidates goal_candidates{}; { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); goal_searcher_->setPlannerData(planner_data_); goal_candidates = status_.get_goal_candidates(); goal_searcher_->update(goal_candidates); @@ -640,7 +630,7 @@ void GoalPlannerModule::selectSafePullOverPath() // found safe pull over path { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_is_safe_static_objects(true); status_.set_pull_over_path(std::make_shared(pull_over_path)); status_.set_current_path_idx(0); @@ -681,7 +671,7 @@ void GoalPlannerModule::selectSafePullOverPath() void GoalPlannerModule::setLanes() { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_current_lanes(utils::getExtendedCurrentLanes( planner_data_, parameters_->backward_goal_search_length, parameters_->forward_goal_search_length, @@ -731,7 +721,7 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_prev_is_safe(status_.get_is_safe_static_objects()); status_.set_prev_is_safe_dynamic_objects( parameters_->safety_check_params.enable_safety_check ? isSafePath() : true); @@ -742,7 +732,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) if (status_.get_prev_is_safe() || !status_.get_prev_stop_path()) { // safe -> not_safe or no prev_stop_path: generate new stop_path output.path = std::make_shared(generateStopPath()); - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_prev_stop_path(output.path); // set stop path as pull over path auto stop_pull_over_path = std::make_shared(); @@ -916,7 +906,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() if (status_.get_has_decided_path()) { if (isActivated() && isWaitingApproval()) { { - const auto transaction = status_.startTransaction(); + const std::lock_guard lock(mutex_); status_.set_last_approved_time(std::make_shared(clock_->now())); status_.set_last_approved_pose( std::make_shared(planner_data_->self_odometry->pose.pose)); @@ -1226,7 +1216,7 @@ bool GoalPlannerModule::isStopped( bool GoalPlannerModule::isStopped() { - const std::lock_guard lock(mutex_); + const std::lock_guard lock(mutex_); return isStopped(odometry_buffer_stopped_, parameters_->th_stopped_time); } @@ -1236,7 +1226,6 @@ bool GoalPlannerModule::isStuck() return false; } - const std::lock_guard lock(mutex_); constexpr double stuck_time = 5.0; if (!isStopped(odometry_buffer_stuck_, stuck_time)) { return false; From 8a66cbae0ac31735bc7ccf915818dd5a631349ec Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 24 Oct 2023 09:29:32 +0900 Subject: [PATCH 155/206] fix(crosswalk): fix duplicated crosswalk launch (#5383) Signed-off-by: Takayuki Murooka --- .../src/manager.cpp | 21 ++++++++++++------- .../src/scene_crosswalk.cpp | 8 +++---- .../src/scene_crosswalk.hpp | 4 ++-- 3 files changed, 19 insertions(+), 14 deletions(-) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 94e87d0174193..dd8dc95a8ad3c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -128,8 +128,9 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto launch = [this, &path](const auto id, const auto & use_regulatory_element) { - if (isModuleRegistered(id)) { + const auto launch = [this, &path]( + const auto lane_id, const std::optional & reg_elem_id) { + if (isModuleRegistered(lane_id)) { return; } @@ -137,24 +138,28 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto logger = logger_.get_child("crosswalk_module"); const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); + // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case + // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, id, lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); - generateUUID(id); - updateRTCStatus(getUUID(id), true, std::numeric_limits::lowest(), path.header.stamp); + node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_)); + generateUUID(lane_id); + updateRTCStatus( + getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->id(), true); + // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. + launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); } const auto crosswalk_lanelets = getCrosswalksOnPath( planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk.id(), false); + launch(crosswalk.id(), std::nullopt); } } @@ -172,7 +177,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); for (const auto & crosswalk : crosswalk_leg_elem_map) { - crosswalk_id_set.insert(crosswalk.first->id()); + crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } return [crosswalk_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index fe96985743b94..56f5072a2ade9 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -176,20 +176,20 @@ tier4_debug_msgs::msg::StringStamped createStringStampedMessage( } // namespace CrosswalkModule::CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), - use_regulatory_element_(use_regulatory_element) + use_regulatory_element_(reg_elem_id) { velocity_factor_.init(VelocityFactor::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( - lanelet_map_ptr->regulatoryElementLayer.get(module_id)); + lanelet_map_ptr->regulatoryElementLayer.get(*reg_elem_id)); stop_lines_ = reg_elem_ptr->stopLines(); crosswalk_ = reg_elem_ptr->crosswalkLanelet(); } else { diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 063ea4f83cd45..0768101179857 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -299,8 +299,8 @@ class CrosswalkModule : public SceneModuleInterface }; CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const PlannerParam & planner_param, const bool use_regulatory_element, + rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; From 7c3bde1a6bdcda425999b5f02f06ffe5712fc709 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Tue, 24 Oct 2023 09:33:04 +0900 Subject: [PATCH 156/206] docs(mpc_lateral_controller): update parameter description (#5309) * update parameter Signed-off-by: kyoichi-sugahara * fix description Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- control/mpc_lateral_controller/README.md | 169 +++++++++++++---------- 1 file changed, 96 insertions(+), 73 deletions(-) diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 7579e3da265c4..50e9d44a0a7a4 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -67,7 +67,7 @@ Set the following from the [controller_node](../trajectory_follower_node/README. - `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering +- `autoware_auto_vehicle_msgs/SteeringReport`: current steering ### Outputs @@ -89,83 +89,108 @@ can be calculated by providing the current steer, velocity, and pose to function The default parameters defined in `param/lateral_controller_defaults.param.yaml` are adjusted to the AutonomouStuff Lexus RX 450h for under 40 km/h driving. -| Name | Type | Description | Default value | -| :------------------------------------------- | :----- | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| show_debug_info | bool | display debug info | false | -| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | -| enable_path_smoothing | bool | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | true | -| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 35 | -| path_smoothing_times | int | number of times of applying path smoothing filter | 1 | -| curvature_smoothing_num_ref_steer | double | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35 | -| curvature_smoothing_num_traj | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 1 | -| extend_trajectory_for_end_yaw_control | bool | trajectory extending flag for end yaw control. | true | -| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | -| admissible_position_error | double | stop vehicle when following position error is larger than this value [m]. | 5.0 | -| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad]. | 1.57 | -| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.0 | -| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.0 | -| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | -| keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | -| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | -| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | -| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.3 | - -(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. - -#### MPC algorithm - -| Name | Type | Description | Default value | -| :-------------------------------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------- | -| qp_solver_type | string | QP solver option. described below in detail. | unconstraint_fast | -| mpc_vehicle_model_type | string | vehicle model option. described below in detail. | kinematics | -| mpc_prediction_horizon | int | total prediction step for MPC | 70 | -| mpc_prediction_sampling_time | double | prediction period for one step [s] | 0.1 | -| mpc_weight_lat_error | double | weight for lateral error | 0.1 | -| mpc_weight_heading_error | double | weight for heading error | 0.0 | -| mpc_weight_heading_error_squared_vel_coeff | double | weight for heading error \* velocity | 5.0 | -| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | -| mpc_weight_steering_input_squared_vel_coeff | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | -| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | -| mpc_weight_terminal_lat_error | double | terminal cost weight for lateral error | 1.0 | -| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | -| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.0 | -| mpc_weight_terminal_heading_error | double | terminal cost weight for heading error | 0.1 | -| mpc_low_curvature_thresh_curvature | double | trajectory curvature threshold to change the weight. If the curvature is lower than this value, the `low_curvature_weight_**` values will be used. | 0.0 | -| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.0 | -| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | -| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.0 | -| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 0.0 | -| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.0 | -| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | -| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | -| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.0 | -| mpc_zero_ff_steer_deg | double | threshold of feedforward angle [deg]. feedforward angle smaller than this value is set to zero. | 2.0 | - -#### Steering offset remover - -Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. - -| Name | Type | Description | Default value | -| :---------------------------------- | :----- | :----------------------------------------------------------------------------------------------- | :------------ | -| enable_auto_steering_offset_removal | bool | Estimate the steering offset and apply compensation | true | -| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation. | 5.56 | -| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | -| average_num | double | The average of this number of data is used as a steering offset. | 1000 | -| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | - -#### Vehicle model +#### System + +| Name | Type | Description | Default value | +| :------------------------ | :------ | :-------------------------------------------------------------------------- | :------------ | +| traj_resample_dist | double | distance of waypoints in resampling [m] | 0.1 | +| use_steer_prediction | boolean | flag for using steer prediction (do not use steer measurement) | false | +| admissible_position_error | double | stop vehicle when following position error is larger than this value [m] | 5.0 | +| admissible_yaw_error_rad | double | stop vehicle when following yaw angle error is larger than this value [rad] | 1.57 | + +#### Path Smoothing + +| Name | Type | Description | Default value | +| :-------------------------------- | :------ | :--------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_path_smoothing | boolean | path smoothing flag. This should be true when uses path resampling to reduce resampling noise. | false | +| path_filter_moving_ave_num | int | number of data points moving average filter for path smoothing | 25 | +| curvature_smoothing_num_traj | int | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | +| curvature_smoothing_num_ref_steer | int | index distance of points used in curvature calculation for reference steering command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 15 | + +#### Trajectory Extending + +| Name | Type | Description | Default value | +| :------------------------------------ | :------ | :-------------------------------------------- | :------------ | +| extend_trajectory_for_end_yaw_control | boolean | trajectory extending flag for end yaw control | true | + +#### MPC Optimization + +| Name | Type | Description | Default value | +| :-------------------------------------------------- | :----- | :----------------------------------------------------------------------------------------------------------- | :------------ | +| qp_solver_type | string | QP solver option. described below in detail. | "osqp" | +| mpc_prediction_horizon | int | total prediction step for MPC | 50 | +| mpc_prediction_dt | double | prediction period for one step [s] | 0.1 | +| mpc_weight_lat_error | double | weight for lateral error | 1.0 | +| mpc_weight_heading_error | double | weight for heading error | 0.0 | +| mpc_weight_heading_error_squared_vel | double | weight for heading error \* velocity | 0.3 | +| mpc_weight_steering_input | double | weight for steering error (steer command - reference steer) | 1.0 | +| mpc_weight_steering_input_squared_vel | double | weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_weight_lat_jerk | double | weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.1 | +| mpc_weight_steer_rate | double | weight for steering rate [rad/s] | 0.0 | +| mpc_weight_steer_acc | double | weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_weight_lat_error | double | [used in a low curvature trajectory] weight for lateral error | 0.1 | +| mpc_low_curvature_weight_heading_error | double | [used in a low curvature trajectory] weight for heading error | 0.0 | +| mpc_low_curvature_weight_heading_error_squared_vel | double | [used in a low curvature trajectory] weight for heading error \* velocity | 0.3 | +| mpc_low_curvature_weight_steering_input | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) | 1.0 | +| mpc_low_curvature_weight_steering_input_squared_vel | double | [used in a low curvature trajectory] weight for steering error (steer command - reference steer) \* velocity | 0.25 | +| mpc_low_curvature_weight_lat_jerk | double | [used in a low curvature trajectory] weight for lateral jerk (steer(i) - steer(i-1)) \* velocity | 0.0 | +| mpc_low_curvature_weight_steer_rate | double | [used in a low curvature trajectory] weight for steering rate [rad/s] | 0.0 | +| mpc_low_curvature_weight_steer_acc | double | [used in a low curvature trajectory] weight for derivatives of the steering rate [rad/ss] | 0.000001 | +| mpc_low_curvature_thresh_curvature | double | threshold of curvature to use "low_curvature" parameter | 0.0 | +| mpc_weight_terminal_lat_error | double | terminal lateral error weight in matrix Q to improve mpc stability | 1.0 | +| mpc_weight_terminal_heading_error | double | terminal heading error weight in matrix Q to improve mpc stability | 0.1 | +| mpc_zero_ff_steer_deg | double | threshold that feed-forward angle becomes zero | 0.5 | +| mpc_acceleration_limit | double | limit on the vehicle's acceleration | 2.0 | +| mpc_velocity_time_constant | double | time constant used for velocity smoothing | 0.3 | +| mpc_min_prediction_length | double | minimum prediction length | 5.0 | + +#### Vehicle Model | Name | Type | Description | Default value | | :----------------------------------- | :------- | :--------------------------------------------------------------------------------- | :------------------- | +| vehicle_model_type | string | vehicle model type for mpc prediction | "kinematics" | | input_delay | double | steering input delay time for delay compensation | 0.24 | -| vehicle_model_steer_tau | double | steering dynamics time constant | 0.3 | -| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [10.0, 20.0, 30.0] | +| vehicle_model_steer_tau | double | steering dynamics time constant (1d approximation) [s] | 0.3 | +| steer_rate_lim_dps_list_by_curvature | [double] | steering angle rate limit list depending on curvature [deg/s] | [40.0, 50.0, 60.0] | | curvature_list_for_steer_rate_lim | [double] | curvature list for steering angle rate limit interpolation in ascending order [/m] | [0.001, 0.002, 0.01] | -| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [40.0, 30.0, 20.0] | +| steer_rate_lim_dps_list_by_velocity | [double] | steering angle rate limit list depending on velocity [deg/s] | [60.0, 50.0, 40.0] | | velocity_list_for_steer_rate_lim | [double] | velocity list for steering angle rate limit interpolation in ascending order [m/s] | [10.0, 15.0, 20.0] | | acceleration_limit | double | acceleration limit for trajectory velocity modification [m/ss] | 2.0 | | velocity_time_constant | double | velocity dynamics time constant for trajectory velocity modification [s] | 0.3 | +#### Lowpass Filter for Noise Reduction + +| Name | Type | Description | Default value | +| :------------------------ | :----- | :------------------------------------------------------------------ | :------------ | +| steering_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for steering output command [hz] | 3.0 | +| error_deriv_lpf_cutoff_hz | double | cutoff frequency of lowpass filter for error derivative [Hz] | 5.0 | + +#### Stop State + +| Name | Type | Description | Default value | +| :------------------------------------------- | :------ | :---------------------------------------------------------------------------------------------- | :------------ | +| stop_state_entry_ego_speed \*1 | double | threshold value of the ego vehicle speed used to the stop state entry condition | 0.001 | +| stop_state_entry_target_speed \*1 | double | threshold value of the target speed used to the stop state entry condition | 0.001 | +| converged_steer_rad | double | threshold value of the steer convergence | 0.1 | +| keep_steer_control_until_converged | boolean | keep steer control until steer is converged | true | +| new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | +| new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | +| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.01 | + +(\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. + +#### Steer Offset + +Defined in the `steering_offset` namespace. This logic is designed as simple as possible, with minimum design parameters. + +| Name | Type | Description | Default value | +| :---------------------------------- | :------ | :----------------------------------------------------------------------------------------------- | :------------ | +| enable_auto_steering_offset_removal | boolean | Estimate the steering offset and apply compensation | true | +| update_vel_threshold | double | If the velocity is smaller than this value, the data is not used for the offset estimation | 5.56 | +| update_steer_threshold | double | If the steering angle is larger than this value, the data is not used for the offset estimation. | 0.035 | +| average_num | int | The average of this number of data is used as a steering offset. | 1000 | +| steering_offset_limit | double | The angle limit to be applied to the offset compensation. | 0.02 | + ##### For dynamics model (WIP) | Name | Type | Description | Default value | @@ -215,10 +240,8 @@ If you want to adjust the effect only in the high-speed range, you can use `weig - `weight_steering_input`: Reduce oscillation of tracking. - `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range. - `weight_lat_jerk`: Reduce lateral jerk. -- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for - stability. -- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` - for stability. +- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. +- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. #### Other tips for tuning From 763a9b842affd2752edd2dd783ebddcb7f385644 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Tue, 24 Oct 2023 11:52:24 +0900 Subject: [PATCH 157/206] feat(behavior_velocity_run_out): ignore momentary detection caused by false positive (#5359) * feat(behavior_velocity_run_out): ignore momentary detection caused by false positive Signed-off-by: Tomohito Ando * style(pre-commit): autofix --------- Signed-off-by: Tomohito Ando Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../README.md | 5 ++++ .../config/run_out.param.yaml | 5 ++++ .../src/manager.cpp | 7 ++++++ .../src/scene.cpp | 25 ++++++++++++++++++- .../src/scene.hpp | 6 +++-- .../src/utils.hpp | 7 ++++++ 6 files changed, 52 insertions(+), 3 deletions(-) diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index ee762653245c6..a36cfdf6485c6 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -217,6 +217,11 @@ You can choose whether to use this feature by parameter of `slow_down_limit.enab | `max_jerk` | double | [m/s^3] minimum jerk deceleration for safe brake. | | `max_acc` | double | [m/s^2] minimum accel deceleration for safe brake. | +| Parameter /ignore_momentary_detection | Type | Description | +| ------------------------------------- | ------ | ----------------------------------------------------------------- | +| `enable` | bool | [-] whether to ignore momentary detection | +| `time_threshold` | double | [sec] ignores detections that persist for less than this duration | + ### Future extensions / Unimplemented parts - Calculate obstacle's min velocity and max velocity from covariance diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml index f9668549f2fb2..2641214ac5ad4 100644 --- a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml +++ b/planning/behavior_velocity_run_out_module/config/run_out.param.yaml @@ -48,3 +48,8 @@ enable: true max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. + + # prevent abrupt stops caused by false positives in perception + ignore_momentary_detection: + enable: true + time_threshold: 0.15 # [sec] ignores detections that persist for less than this duration diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index b262bf00cb57a..a5253f59b60f9 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -119,6 +119,13 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) p.max_acc = getOrDeclareParameter(node, ns_m + ".max_acc"); } + { + auto & p = planner_param_.ignore_momentary_detection; + const std::string ns_param = ns + ".ignore_momentary_detection"; + p.enable = getOrDeclareParameter(node, ns_param + ".enable"); + p.time_threshold = getOrDeclareParameter(node, ns_param + ".time_threshold"); + } + debug_ptr_ = std::make_shared(node); setDynamicObstacleCreator(node, debug_ptr_); } diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index b91d1d1fe1ae7..54902f52c47da 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -137,7 +137,7 @@ bool RunOutModule::modifyPathVelocity( } boost::optional RunOutModule::detectCollision( - const std::vector & dynamic_obstacles, const PathWithLaneId & path) const + const std::vector & dynamic_obstacles, const PathWithLaneId & path) { if (path.points.size() < 2) { RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); @@ -181,6 +181,11 @@ boost::optional RunOutModule::detectCollision( continue; } + // ignore momentary obstacle detection to avoid sudden stopping by false positive + if (isMomentaryDetection()) { + return {}; + } + debug_ptr_->pushCollisionPoints(obstacle_selected->collision_points); debug_ptr_->pushNearestCollisionPoint(obstacle_selected->nearest_collision_point); @@ -188,6 +193,7 @@ boost::optional RunOutModule::detectCollision( } // no collision detected + first_detected_time_.reset(); return {}; } @@ -812,4 +818,21 @@ void RunOutModule::publishDebugValue( debug_ptr_->publishDebugValue(); } +bool RunOutModule::isMomentaryDetection() +{ + if (!planner_param_.ignore_momentary_detection.enable) { + return false; + } + + if (!first_detected_time_) { + first_detected_time_ = std::make_shared(clock_->now()); + return true; + } + + const auto now = clock_->now(); + const double elapsed_time_since_detection = (now - *first_detected_time_).seconds(); + RCLCPP_DEBUG_STREAM(logger_, "elapsed_time_since_detection: " << elapsed_time_since_detection); + + return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold; +} } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 20bd8ffc799ff..925d8ea8b234c 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -62,13 +62,13 @@ class RunOutModule : public SceneModuleInterface std::unique_ptr dynamic_obstacle_creator_; std::shared_ptr debug_ptr_; std::unique_ptr state_machine_; + std::shared_ptr first_detected_time_; // Function Polygons2d createDetectionAreaPolygon(const PathWithLaneId & smoothed_path) const; boost::optional detectCollision( - const std::vector & dynamic_obstacles, - const PathWithLaneId & path_points) const; + const std::vector & dynamic_obstacles, const PathWithLaneId & path_points); float calcCollisionPositionOfVehicleSide( const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; @@ -141,6 +141,8 @@ class RunOutModule : public SceneModuleInterface const PathWithLaneId & path, const std::vector extracted_obstacles, const boost::optional & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose) const; + + bool isMomentaryDetection(); }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 8fc3d48de4858..b25a8fc94bff3 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -126,6 +126,12 @@ struct DynamicObstacleParam float points_interval; // [m] }; +struct IgnoreMomentaryDetection +{ + bool enable; + double time_threshold; +}; + struct PlannerParam { CommonParam common; @@ -138,6 +144,7 @@ struct PlannerParam DynamicObstacleParam dynamic_obstacle; SlowDownLimit slow_down_limit; Smoother smoother; + IgnoreMomentaryDetection ignore_momentary_detection; }; enum class DetectionMethod { From 0f9eb8af887b1f72611b60b03fed41013556e356 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 24 Oct 2023 14:13:29 +0900 Subject: [PATCH 158/206] feat(imu_corrector): changed input topic of GyroBiasEstimator from ndt_pose to ekf_odom (#5374) Changed input topic of GyroBiasEstimator from pose to odom Signed-off-by: Shintaro Sakoda --- .../launch/gyro_bias_estimator.launch.xml | 4 ++-- sensing/imu_corrector/src/gyro_bias_estimator.cpp | 13 ++++++------- sensing/imu_corrector/src/gyro_bias_estimator.hpp | 6 ++++-- 3 files changed, 12 insertions(+), 11 deletions(-) diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml index 0d7cba9faa3a6..e16ccef446aba 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -1,14 +1,14 @@ - + - + diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 21bb51dc5f1f1..50e4a702ec949 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -43,9 +43,9 @@ GyroBiasEstimator::GyroBiasEstimator() imu_sub_ = create_subscription( "~/input/imu_raw", rclcpp::SensorDataQoS(), [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); - pose_sub_ = create_subscription( - "~/input/pose", rclcpp::SensorDataQoS(), - [this](const PoseWithCovarianceStamped::ConstSharedPtr msg) { callback_pose(msg); }); + odom_sub_ = create_subscription( + "~/input/odom", rclcpp::SensorDataQoS(), + [this](const Odometry::ConstSharedPtr msg) { callback_odom(msg); }); gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); auto timer_callback = std::bind(&GyroBiasEstimator::timer_callback, this); @@ -88,12 +88,11 @@ void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) } } -void GyroBiasEstimator::callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr) +void GyroBiasEstimator::callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr) { - // push pose_msg to queue geometry_msgs::msg::PoseStamped pose; - pose.header = pose_msg_ptr->header; - pose.pose = pose_msg_ptr->pose.pose; + pose.header = odom_msg_ptr->header; + pose.pose = odom_msg_ptr->pose.pose; pose_buf_.push_back(pose); } diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 7eb06bcdcb365..821cba0b213ff 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -38,6 +39,7 @@ class GyroBiasEstimator : public rclcpp::Node using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; using Vector3 = geometry_msgs::msg::Vector3; + using Odometry = nav_msgs::msg::Odometry; public: GyroBiasEstimator(); @@ -45,7 +47,7 @@ class GyroBiasEstimator : public rclcpp::Node private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); - void callback_pose(const PoseWithCovarianceStamped::ConstSharedPtr pose_msg_ptr); + void callback_odom(const Odometry::ConstSharedPtr odom_msg_ptr); void timer_callback(); static geometry_msgs::msg::Vector3 transform_vector3( @@ -55,7 +57,7 @@ class GyroBiasEstimator : public rclcpp::Node const std::string output_frame_ = "base_link"; rclcpp::Subscription::SharedPtr imu_sub_; - rclcpp::Subscription::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Publisher::SharedPtr gyro_bias_pub_; rclcpp::TimerBase::SharedPtr timer_; From 91481daab67f95623894a37cec231e7d42661b1d Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 24 Oct 2023 15:15:01 +0900 Subject: [PATCH 159/206] chore(processing_time_checker): update for windows size and rich display (#5389) Signed-off-by: Takamasa Horibe --- .../scripts/processing_time_checker.py | 71 ++++++++++++++++--- 1 file changed, 61 insertions(+), 10 deletions(-) diff --git a/planning/planning_debug_tools/scripts/processing_time_checker.py b/planning/planning_debug_tools/scripts/processing_time_checker.py index 3b80ce4e5588f..fa6bc57fb7ae5 100755 --- a/planning/planning_debug_tools/scripts/processing_time_checker.py +++ b/planning/planning_debug_tools/scripts/processing_time_checker.py @@ -16,6 +16,7 @@ import argparse +from collections import deque import os import sys @@ -25,11 +26,28 @@ class ProcessingTimeSubscriber(Node): - def __init__(self, max_display_time=150, display_frequency=5.0): + def __init__( + self, + max_display_time=150, + display_frequency=5.0, + window_size=1, + bar_scale=2, + display_character="|", + ): super().__init__("processing_time_subscriber") self.data_map = {} # {topic_name: data_value} + self.data_queue_map = {} # {topic_name: deque} + self.window_size = window_size self.max_display_time = max_display_time + self.bar_scale = bar_scale + self.display_character = display_character + + # Initialize a set to keep track of subscribed topics + self.subscribed_topics = set() + + # Call get_topic_list immediately and set a timer to call it regularly self.get_topic_list() + self.create_timer(1.0, self.get_topic_list) # update topic list every 1 second # Timer to print data at given frequency self.create_timer(1.0 / display_frequency, self.print_data) @@ -40,14 +58,22 @@ def get_topic_list(self): # Subscribe to topics with 'processing_time_ms' suffix for topic, types in topic_list: - if topic.endswith("processing_time_ms"): + if topic.endswith("processing_time_ms") and topic not in self.subscribed_topics: self.create_subscription( Float64Stamped, topic, lambda msg, topic=topic: self.callback(msg, topic), 1 ) self.get_logger().info(f"Subscribed to {topic} | Type: {types}") + self.subscribed_topics.add(topic) def callback(self, msg, topic): - self.data_map[topic] = msg.data + # Add the new data to the queue + if topic not in self.data_queue_map: + self.data_queue_map[topic] = deque(maxlen=self.window_size) + self.data_queue_map[topic].append(msg.data) + + # Calculate the average + avg_value = sum(self.data_queue_map[topic]) / len(self.data_queue_map[topic]) + self.data_map[topic] = avg_value def print_data(self): # Clear terminal @@ -61,9 +87,9 @@ def print_data(self): max_topic_length = max(map(len, self.data_map.keys())) # Generate the header based on the max_display_time - header_str = "topics".ljust(max_topic_length) + ":" - for i in range(0, self.max_display_time + 1, 20): - header_str += f" {i}ms".ljust(20) + header_str = f"topics (window size = {self.window_size})".ljust(max_topic_length) + ":" + for i in range(0, self.max_display_time + 1, 10): + header_str += f" {i}ms".ljust(self.bar_scale * 10) # Print the header print(header_str) @@ -74,8 +100,12 @@ def print_data(self): # Round the data to the third decimal place for display data_rounded = round(data, 3) # Calculate the number of bars to be displayed (clamped to max_display_time) - num_bars = int(min(data, self.max_display_time - 1)) + 1 - print(f"{topic.ljust(max_topic_length)}: {'|' * num_bars} ({data_rounded}ms)") + num_bars = ( + int(min(data * self.bar_scale, self.max_display_time * self.bar_scale - 1)) + 1 + ) # Convert bar_scale's reciprocal to milliseconds + print( + f"{topic.ljust(max_topic_length)}: {self.display_character * num_bars} [{data_rounded}ms]" + ) def main(args=None): @@ -84,16 +114,37 @@ def main(args=None): parser = argparse.ArgumentParser(description="Processing Time Subscriber Parameters") parser.add_argument( - "-m", "--max_display_time", type=int, default=150, help="Maximum display time in ms" + "-m", "--max_display_time", type=int, default=50, help="Maximum display time in ms" ) parser.add_argument( "-f", "--display_frequency", type=float, default=5.0, help="Display frequency in Hz" ) + parser.add_argument( + "-w", "--window_size", type=int, default=5, help="Number of samples to average" + ) + parser.add_argument( + "-bs", + "--bar_scale", + type=int, + default=2, + help="Number of bars per second. Default is 2 bars per second.", + ) + parser.add_argument( + "-dc", + "--display_character", + type=str, + default="|", + help="Character to use for the display. Default is '|'.", + ) args = parser.parse_args() rclpy.init(args=cmd_args) # Use the original command line arguments here subscriber = ProcessingTimeSubscriber( - max_display_time=args.max_display_time, display_frequency=args.display_frequency + max_display_time=args.max_display_time, + display_frequency=args.display_frequency, + window_size=args.window_size, + bar_scale=args.bar_scale, + display_character=args.display_character, ) rclpy.spin(subscriber) subscriber.destroy_node() From 189438e2729e5152d58b8ac7357c5c7202ec5b98 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 24 Oct 2023 15:16:56 +0900 Subject: [PATCH 160/206] refactor(topic_state_monitor): add state log message (#5378) * refactor(topic_state_monitor): add state log message Signed-off-by: Takamasa Horibe * add debug print Signed-off-by: Takamasa Horibe --------- Signed-off-by: Takamasa Horibe --- system/default_ad_api/src/operation_mode.cpp | 18 ++++++++++++++---- system/default_ad_api/src/operation_mode.hpp | 3 ++- .../src/topic_state_monitor_core.cpp | 11 +++++++++++ 3 files changed, 27 insertions(+), 5 deletions(-) diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp index a61a1b75697ab..829585ed4b8b4 100644 --- a/system/default_ad_api/src/operation_mode.cpp +++ b/system/default_ad_api/src/operation_mode.cpp @@ -46,12 +46,11 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) for (size_t i = 0; i < module_names.size(); ++i) { const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; const auto qos = rclcpp::QoS(1).transient_local(); - const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { - module_states_[i] = msg->available; + const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) { + module_states_[module_names[i]] = msg->available; }; sub_module_states_.push_back(create_subscription(name, qos, callback)); } - module_states_.resize(module_names.size()); timer_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); @@ -137,11 +136,22 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP void OperationModeNode::on_timer() { bool autonomous_available = true; + std::string unhealthy_components = ""; for (const auto & state : module_states_) { - autonomous_available &= state; + if (!state.second) { + unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first; + } + autonomous_available &= state.second; } mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; + if (!unhealthy_components.empty()) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 3000, + "%s component state is unhealthy. Autonomous is not available.", + unhealthy_components.c_str()); + } + update_state(); } diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp index 5f5699f42cab7..1830b7041b566 100644 --- a/system/default_ad_api/src/operation_mode.hpp +++ b/system/default_ad_api/src/operation_mode.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -65,7 +66,7 @@ class OperationModeNode : public rclcpp::Node Cli cli_mode_; Cli cli_control_; - std::vector module_states_; + std::unordered_map module_states_; std::vector::SharedPtr> sub_module_states_; void on_change_to_stop( diff --git a/system/topic_state_monitor/src/topic_state_monitor_core.cpp b/system/topic_state_monitor/src/topic_state_monitor_core.cpp index 326193a58dc4e..68fa3764d8ecd 100644 --- a/system/topic_state_monitor/src/topic_state_monitor_core.cpp +++ b/system/topic_state_monitor/src/topic_state_monitor_core.cpp @@ -151,6 +151,13 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu stat.addf("topic", "%s", node_param_.topic.c_str()); } + const auto print_warn = [&](const std::string & msg) { + RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + const auto print_debug = [&](const std::string & msg) { + RCLCPP_DEBUG_THROTTLE(get_logger(), *get_clock(), 3000, "%s", msg.c_str()); + }; + // Judge level int8_t level = DiagnosticStatus::OK; if (topic_status == TopicStatus::Ok) { @@ -159,15 +166,19 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu } else if (topic_status == TopicStatus::NotReceived) { level = DiagnosticStatus::ERROR; stat.add("status", "NotReceived"); + print_debug(node_param_.topic + " has not received."); } else if (topic_status == TopicStatus::WarnRate) { level = DiagnosticStatus::WARN; stat.add("status", "WarnRate"); + print_warn(node_param_.topic + " topic rate has dropped to the warning level."); } else if (topic_status == TopicStatus::ErrorRate) { level = DiagnosticStatus::ERROR; stat.add("status", "ErrorRate"); + print_warn(node_param_.topic + " topic rate has dropped to the error level."); } else if (topic_status == TopicStatus::Timeout) { level = DiagnosticStatus::ERROR; stat.add("status", "Timeout"); + print_warn(node_param_.topic + " topic is timeout."); } // Add key-value From cc0b202353b8ef55cb5f54f9ade446bdd7d7a242 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Tue, 24 Oct 2023 16:26:06 +0900 Subject: [PATCH 161/206] refactor(localization): add localization_util package (#5377) * Added localization_util Signed-off-by: Shintaro Sakoda * Fixed library Signed-off-by: Shintaro Sakoda * Separated a package tree_structured_parzen_estimator Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Fixed library Signed-off-by: Shintaro Sakoda * Added maintainer Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- localization/localization_util/CMakeLists.txt | 15 +++++++ localization/localization_util/README.md | 5 +++ .../localization_util}/matrix_type.hpp | 6 +-- .../pose_array_interpolator.hpp | 8 ++-- .../tf2_listener_module.hpp | 6 +-- .../include/localization_util}/util_func.hpp | 6 +-- localization/localization_util/package.xml | 40 +++++++++++++++++++ .../src/pose_array_interpolator.cpp | 2 +- .../src/tf2_listener_module.cpp | 2 +- .../src/util_func.cpp | 4 +- localization/ndt_scan_matcher/CMakeLists.txt | 12 ------ .../ndt_scan_matcher/map_update_module.hpp | 4 +- .../ndt_scan_matcher_core.hpp | 2 +- localization/ndt_scan_matcher/package.xml | 2 + localization/ndt_scan_matcher/src/debug.cpp | 2 +- .../src/ndt_scan_matcher_core.cpp | 8 ++-- .../CMakeLists.txt | 23 +++++++++++ .../README.md | 5 +++ .../tree_structured_parzen_estimator.hpp | 6 +-- .../package.xml | 23 +++++++++++ .../src/tree_structured_parzen_estimator.cpp | 2 +- .../test/test_tpe.cpp | 2 +- 22 files changed, 143 insertions(+), 42 deletions(-) create mode 100644 localization/localization_util/CMakeLists.txt create mode 100644 localization/localization_util/README.md rename localization/{ndt_scan_matcher/include/ndt_scan_matcher => localization_util/include/localization_util}/matrix_type.hpp (84%) rename localization/{ndt_scan_matcher/include/ndt_scan_matcher => localization_util/include/localization_util}/pose_array_interpolator.hpp (90%) rename localization/{ndt_scan_matcher/include/ndt_scan_matcher => localization_util/include/localization_util}/tf2_listener_module.hpp (88%) rename localization/{ndt_scan_matcher/include/ndt_scan_matcher => localization_util/include/localization_util}/util_func.hpp (96%) create mode 100644 localization/localization_util/package.xml rename localization/{ndt_scan_matcher => localization_util}/src/pose_array_interpolator.cpp (98%) rename localization/{ndt_scan_matcher => localization_util}/src/tf2_listener_module.cpp (97%) rename localization/{ndt_scan_matcher => localization_util}/src/util_func.cpp (99%) create mode 100644 localization/tree_structured_parzen_estimator/CMakeLists.txt create mode 100644 localization/tree_structured_parzen_estimator/README.md rename localization/{ndt_scan_matcher/include/ndt_scan_matcher => tree_structured_parzen_estimator/include/tree_structured_parzen_estimator}/tree_structured_parzen_estimator.hpp (90%) create mode 100644 localization/tree_structured_parzen_estimator/package.xml rename localization/{ndt_scan_matcher => tree_structured_parzen_estimator}/src/tree_structured_parzen_estimator.cpp (98%) rename localization/{ndt_scan_matcher => tree_structured_parzen_estimator}/test/test_tpe.cpp (96%) diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt new file mode 100644 index 0000000000000..9490ffb67723b --- /dev/null +++ b/localization/localization_util/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.14) +project(localization_util) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(localization_util SHARED + src/util_func.cpp + src/pose_array_interpolator.cpp + src/tf2_listener_module.cpp +) + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md new file mode 100644 index 0000000000000..a3e980464d0c6 --- /dev/null +++ b/localization/localization_util/README.md @@ -0,0 +1,5 @@ +# localization_util + +`localization_util`` is a localization utility package. + +This package does not have a node, it is just a library. diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp b/localization/localization_util/include/localization_util/matrix_type.hpp similarity index 84% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp rename to localization/localization_util/include/localization_util/matrix_type.hpp index 038ed4549eb2f..d9ec9b369127a 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/matrix_type.hpp +++ b/localization/localization_util/include/localization_util/matrix_type.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ -#define NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ +#ifndef LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ +#define LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ #include using Matrix6d = Eigen::Matrix; using RowMatrixXd = Eigen::Matrix; -#endif // NDT_SCAN_MATCHER__MATRIX_TYPE_HPP_ +#endif // LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp b/localization/localization_util/include/localization_util/pose_array_interpolator.hpp similarity index 90% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp rename to localization/localization_util/include/localization_util/pose_array_interpolator.hpp index f1e7dfb3f544b..998d8465733f5 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_array_interpolator.hpp +++ b/localization/localization_util/include/localization_util/pose_array_interpolator.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ -#define NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ +#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ +#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" #include @@ -59,4 +59,4 @@ class PoseArrayInterpolator const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; -#endif // NDT_SCAN_MATCHER__POSE_ARRAY_INTERPOLATOR_HPP_ +#endif // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp b/localization/localization_util/include/localization_util/tf2_listener_module.hpp similarity index 88% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp rename to localization/localization_util/include/localization_util/tf2_listener_module.hpp index 159acbd75ce3d..b332f9effe0e0 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tf2_listener_module.hpp +++ b/localization/localization_util/include/localization_util/tf2_listener_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ -#define NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ +#ifndef LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ +#define LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ #include @@ -40,4 +40,4 @@ class Tf2ListenerModule tf2_ros::TransformListener tf2_listener_; }; -#endif // NDT_SCAN_MATCHER__TF2_LISTENER_MODULE_HPP_ +#endif // LOCALIZATION_UTIL__TF2_LISTENER_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp similarity index 96% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp rename to localization/localization_util/include/localization_util/util_func.hpp index 5c37cef36c422..bd9a2b9305e25 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ -#define NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ +#ifndef LOCALIZATION_UTIL__UTIL_FUNC_HPP_ +#define LOCALIZATION_UTIL__UTIL_FUNC_HPP_ #include #include @@ -91,4 +91,4 @@ void output_pose_with_cov_to_log( const rclcpp::Logger logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); -#endif // NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ +#endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/localization_util/package.xml b/localization/localization_util/package.xml new file mode 100644 index 0000000000000..20f9b160212b5 --- /dev/null +++ b/localization/localization_util/package.xml @@ -0,0 +1,40 @@ + + + + localization_util + 0.1.0 + The localization_util package + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + Apache License 2.0 + Yamato Ando + + ament_cmake_auto + autoware_cmake + + fmt + geometry_msgs + nav_msgs + rclcpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tf2_sensor_msgs + tier4_autoware_utils + tier4_debug_msgs + tier4_localization_msgs + visualization_msgs + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/localization_util/src/pose_array_interpolator.cpp similarity index 98% rename from localization/ndt_scan_matcher/src/pose_array_interpolator.cpp rename to localization/localization_util/src/pose_array_interpolator.cpp index f09b71523e804..ebc904fcf8d38 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/localization_util/src/pose_array_interpolator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/pose_array_interpolator.hpp" +#include "localization_util/pose_array_interpolator.hpp" PoseArrayInterpolator::PoseArrayInterpolator( rclcpp::Node * node, const rclcpp::Time & target_ros_time, diff --git a/localization/ndt_scan_matcher/src/tf2_listener_module.cpp b/localization/localization_util/src/tf2_listener_module.cpp similarity index 97% rename from localization/ndt_scan_matcher/src/tf2_listener_module.cpp rename to localization/localization_util/src/tf2_listener_module.cpp index 364952a631f06..8a19ceec9f30d 100644 --- a/localization/ndt_scan_matcher/src/tf2_listener_module.cpp +++ b/localization/localization_util/src/tf2_listener_module.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "localization_util/tf2_listener_module.hpp" #include diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/localization_util/src/util_func.cpp similarity index 99% rename from localization/ndt_scan_matcher/src/util_func.cpp rename to localization/localization_util/src/util_func.cpp index 52cb7844ab241..865cc02cff293 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/matrix_type.hpp" +#include "localization_util/matrix_type.hpp" static std::random_device seed_gen; diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 955234f95ff18..79491a85a0a66 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -29,12 +29,8 @@ ament_auto_add_executable(ndt_scan_matcher src/debug.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp - src/util_func.cpp - src/pose_array_interpolator.cpp - src/tf2_listener_module.cpp src/map_module.cpp src/map_update_module.cpp - src/tree_structured_parzen_estimator.cpp ) link_directories(${PCL_LIBRARY_DIRS}) @@ -45,14 +41,6 @@ if(BUILD_TESTING) test/test_ndt_scan_matcher_launch.py TIMEOUT "30" ) - - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) - target_include_directories(test_tpe PRIVATE include) - target_link_libraries(test_tpe) endif() ament_auto_package( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 4f630bb8c5898..13721c03ca22e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -15,10 +15,10 @@ #ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#include "localization_util/tf2_listener_module.hpp" +#include "localization_util/util_func.hpp" #include "ndt_scan_matcher/debug.hpp" #include "ndt_scan_matcher/particle.hpp" -#include "ndt_scan_matcher/tf2_listener_module.hpp" -#include "ndt_scan_matcher/util_func.hpp" #include #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 153418e5a8f75..e5f46b5022b51 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,9 +17,9 @@ #define FMT_HEADER_ONLY +#include "localization_util/tf2_listener_module.hpp" #include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" -#include "ndt_scan_matcher/tf2_listener_module.hpp" #include diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 986179b9e02e2..a9083c4ae0ae4 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -19,6 +19,7 @@ fmt geometry_msgs libpcl-all-dev + localization_util nav_msgs ndt_omp pcl_conversions @@ -34,6 +35,7 @@ tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs + tree_structured_parzen_estimator visualization_msgs ament_cmake_cppcheck diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/debug.cpp index 9c82e06d89b80..941f682de5803 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/debug.cpp @@ -14,7 +14,7 @@ #include "ndt_scan_matcher/debug.hpp" -#include "ndt_scan_matcher/util_func.hpp" +#include "localization_util/util_func.hpp" visualization_msgs::msg::MarkerArray make_debug_markers( const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8e583fd3a666b..b38e3f0826184 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -14,11 +14,11 @@ #include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" -#include "ndt_scan_matcher/matrix_type.hpp" +#include "localization_util/matrix_type.hpp" +#include "localization_util/pose_array_interpolator.hpp" +#include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" -#include "ndt_scan_matcher/pose_array_interpolator.hpp" -#include "ndt_scan_matcher/tree_structured_parzen_estimator.hpp" -#include "ndt_scan_matcher/util_func.hpp" +#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" #include #include diff --git a/localization/tree_structured_parzen_estimator/CMakeLists.txt b/localization/tree_structured_parzen_estimator/CMakeLists.txt new file mode 100644 index 0000000000000..7b687a12a003c --- /dev/null +++ b/localization/tree_structured_parzen_estimator/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.14) +project(tree_structured_parzen_estimator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(tree_structured_parzen_estimator SHARED + src/tree_structured_parzen_estimator.cpp +) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_tpe + test/test_tpe.cpp + src/tree_structured_parzen_estimator.cpp + ) + target_include_directories(test_tpe PRIVATE include) + target_link_libraries(test_tpe) +endif() + +ament_auto_package( + INSTALL_TO_SHARE +) diff --git a/localization/tree_structured_parzen_estimator/README.md b/localization/tree_structured_parzen_estimator/README.md new file mode 100644 index 0000000000000..2b21e65929485 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/README.md @@ -0,0 +1,5 @@ +# tree_structured_parzen_estimator + +`tree_structured_parzen_estimator`` is a package for black-box optimization. + +This package does not have a node, it is just a library. diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp similarity index 90% rename from localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp rename to localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp index 3c79ab75dba48..b7b522b4e6b76 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/tree_structured_parzen_estimator.hpp +++ b/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NDT_SCAN_MATCHER__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define NDT_SCAN_MATCHER__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#ifndef TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#define TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ /* A implementation of tree-structured parzen estimator (TPE) @@ -77,4 +77,4 @@ class TreeStructuredParzenEstimator const Input base_stddev_; }; -#endif // NDT_SCAN_MATCHER__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ +#endif // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml new file mode 100644 index 0000000000000..b699d5c69e610 --- /dev/null +++ b/localization/tree_structured_parzen_estimator/package.xml @@ -0,0 +1,23 @@ + + + + tree_structured_parzen_estimator + 0.1.0 + The tree_structured_parzen_estimator package + Yamato Ando + Masahiro Sakamoto + Shintaro Sakoda + Kento Yabuuchi + Apache License 2.0 + Shintaro Sakoda + + ament_cmake_auto + autoware_cmake + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/localization/ndt_scan_matcher/src/tree_structured_parzen_estimator.cpp b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp similarity index 98% rename from localization/ndt_scan_matcher/src/tree_structured_parzen_estimator.cpp rename to localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp index b92c8a075252c..99c70a844f331 100644 --- a/localization/ndt_scan_matcher/src/tree_structured_parzen_estimator.cpp +++ b/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/tree_structured_parzen_estimator.hpp" +#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" #include #include diff --git a/localization/ndt_scan_matcher/test/test_tpe.cpp b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp similarity index 96% rename from localization/ndt_scan_matcher/test/test_tpe.cpp rename to localization/tree_structured_parzen_estimator/test/test_tpe.cpp index fb7190f4a1c74..32eb66e70fb16 100644 --- a/localization/ndt_scan_matcher/test/test_tpe.cpp +++ b/localization/tree_structured_parzen_estimator/test/test_tpe.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/tree_structured_parzen_estimator.hpp" +#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" #include From db2768626a2e60e859934e0155b42088ef592be5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 24 Oct 2023 17:03:54 +0900 Subject: [PATCH 162/206] fix(intersection): wrong argument order (#5386) Signed-off-by: Takayuki Murooka --- planning/behavior_velocity_intersection_module/src/util.cpp | 4 ++-- planning/behavior_velocity_intersection_module/src/util.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 27310f2129937..42412bbccd424 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -269,8 +269,8 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, const double max_accel, - const double max_jerk, const double delay_response_time, + const double stop_line_margin, const double max_accel, const double max_jerk, + const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 125d3bdfb570a..5faacd4325b06 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -70,8 +70,8 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, const double max_accel, - const double max_jerk, const double delay_response_time, + const double stop_line_margin, const double max_accel, const double max_jerk, + const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon( From f400b912c1d0570a94b5fcf969f32fca2739b33a Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 24 Oct 2023 21:39:33 +0900 Subject: [PATCH 163/206] fix(utils): remove sharp angle bound (#5384) * fix(utils): remove sharp angle bound Signed-off-by: satoshi-ota * fix(utils): guard invalid access Signed-off-by: satoshi-ota * chore(utils): add comment Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../behavior_path_planner/src/utils/utils.cpp | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 6927327e24ef4..9b10dfc32457f 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2011,6 +2011,11 @@ void makeBoundLongitudinallyMonotonic( std::vector ret = bound; auto itr = std::next(ret.begin()); while (std::next(itr) != ret.end()) { + if (itr == ret.begin()) { + itr++; + continue; + } + const auto p1 = *std::prev(itr); const auto p2 = *itr; const auto p3 = *std::next(itr); @@ -2024,11 +2029,20 @@ void makeBoundLongitudinallyMonotonic( const auto dist_3to2 = tier4_autoware_utils::calcDistance3d(p3, p2); constexpr double epsilon = 1e-3; + + // Remove overlapped point. + if (dist_1to2 < epsilon || dist_3to2 < epsilon) { + itr = std::prev(ret.erase(itr)); + continue; + } + + // If the angle between the points is sharper than 45 degrees, remove the middle point. if (std::cos(M_PI_4) < product / dist_1to2 / dist_3to2 + epsilon) { - itr = ret.erase(itr); - } else { - itr++; + itr = std::prev(ret.erase(itr)); + continue; } + + itr++; } return ret; From fe650db2ce9dea71c9767cd3f0619c345eaf46bf Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 25 Oct 2023 10:38:52 +0900 Subject: [PATCH 164/206] feat(intersection): check path margin for overshoot vehicles on red light (#5394) --- .../config/intersection.param.yaml | 2 + .../src/debug.cpp | 6 +++ .../src/manager.cpp | 3 ++ .../src/scene_intersection.cpp | 51 ++++++++++++++++++- .../src/scene_intersection.hpp | 4 ++ .../src/util.cpp | 11 +++- .../src/util_type.hpp | 1 + 7 files changed, 74 insertions(+), 4 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index fde55dc7a6c55..82a5f65622d0b 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -56,6 +56,8 @@ object_dist_to_stopline: 10.0 # [m] ignore_on_amber_traffic_light: object_expected_deceleration: 2.0 # [m/ss] + ignore_on_red_traffic_light: + object_margin_to_path: 2.0 occlusion: enable: false diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index f472c4bf51e31..8d9beb34d05ee 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -230,6 +230,12 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( debug_data_.amber_ignore_targets, "amber_ignore_targets", module_id_, now, 0.0, 1.0, 0.0), &debug_marker_array, now); + appendMarkerArray( + debug::createObjectsMarkerArray( + debug_data_.red_overshoot_ignore_targets, "red_overshoot_ignore_targets", module_id_, now, + 0.0, 1.0, 0.0), + &debug_marker_array, now); + appendMarkerArray( debug::createObjectsMarkerArray( debug_data_.stuck_targets, "stuck_targets", module_id_, now, 0.99, 0.99, 0.2), diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index a65e06eedcdf0..3c0d7fa0b4f1c 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -139,6 +139,9 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration = getOrDeclareParameter( node, ns + ".collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration"); + ip.collision_detection.ignore_on_red_traffic_light.object_margin_to_path = + getOrDeclareParameter( + node, ns + ".collision_detection.ignore_on_red_traffic_light.object_margin_to_path"); ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 4def152567b32..53a99a1ee6b4d 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -30,6 +30,7 @@ #include #include +#include #include #include #include @@ -1519,18 +1520,64 @@ bool IntersectionModule::checkCollision( .object_expected_deceleration)); return dist_to_stop_line > braking_distance; }; - + const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { + if ( + !target_object.attention_lanelet || !target_object.dist_to_stop_line || + !target_object.stop_line) { + return false; + } + const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; + const double braking_distance = + v * v / + (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light + .object_expected_deceleration)); + if (dist_to_stop_line > braking_distance) { + return false; + } + const auto stopline_front = target_object.stop_line.value().front(); + const auto stopline_back = target_object.stop_line.value().back(); + tier4_autoware_utils::LineString2d object_line; + object_line.emplace_back( + (stopline_front.x() + stopline_back.x()) / 2.0, + (stopline_front.y() + stopline_back.y()) / 2.0); + const auto stopline_mid = object_line.front(); + const auto endpoint = target_object.attention_lanelet.value().centerline().back(); + object_line.emplace_back(endpoint.x(), endpoint.y()); + std::vector intersections; + bg::intersection(object_line, ego_lane.centerline2d().basicLineString(), intersections); + if (intersections.empty()) { + return false; + } + const auto collision_point = intersections.front(); + // distance from object expected stop position to collision point + const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance; + const double stopline_to_collision = + std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); + const double object2collision = stopline_to_collision - stopline_to_object; + const double margin = + planner_param_.collision_detection.ignore_on_red_traffic_light.object_margin_to_path; + return (object2collision > margin) || (object2collision < 0); + }; // check collision between predicted_path and ego_area bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; // If the vehicle is expected to stop before their stopline, ignore + const bool expected_to_stop_before_stopline = expectedToStopBeforeStopLine(target_object); if ( traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED && - expectedToStopBeforeStopLine(target_object)) { + expected_to_stop_before_stopline) { debug_data_.amber_ignore_targets.objects.push_back(object); continue; } + const bool is_tolerable_overshoot = isTolerableOvershoot(target_object); + if ( + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED && + !expected_to_stop_before_stopline && is_tolerable_overshoot) { + debug_data_.red_overshoot_ignore_targets.objects.push_back(object); + continue; + } for (const auto & predicted_path : object.kinematics.predicted_paths) { if ( predicted_path.confidence < diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 764f5bd7fe058..93fec171ec0d2 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -118,6 +118,10 @@ class IntersectionModule : public SceneModuleInterface { double object_expected_deceleration; } ignore_on_amber_traffic_light; + struct IgnoreOnRedTrafficLight + { + double object_margin_to_path; + } ignore_on_red_traffic_light; } collision_detection; struct Occlusion { diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 42412bbccd424..25eaaf4d8cb9d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -841,9 +841,16 @@ IntersectionLanelets getObjectiveLanelets( result.attention_stop_lines_.push_back(stop_line); } result.attention_non_preceding_ = std::move(detection_lanelets); - // TODO(Mamoru Sobue): find stop lines for attention_non_preceding_ if needed for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - result.attention_non_preceding_stop_lines_.push_back(std::nullopt); + std::optional stop_line = std::nullopt; + const auto & ll = result.attention_non_preceding_.at(i); + const auto traffic_lights = ll.regulatoryElementsAs(); + for (const auto & traffic_light : traffic_lights) { + const auto stop_line_opt = traffic_light->stopLine(); + if (!stop_line_opt) continue; + stop_line = stop_line_opt.get(); + } + result.attention_non_preceding_stop_lines_.push_back(stop_line); } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index fdcf05a97a7a9..73e60aea6471a 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -49,6 +49,7 @@ struct DebugData std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; From cb0279964cb84db290fca6528a97c00d9eb188a7 Mon Sep 17 00:00:00 2001 From: Yuki TAKAGI <141538661+yuki-takagi-66@users.noreply.github.com> Date: Wed, 25 Oct 2023 11:28:24 +0900 Subject: [PATCH 165/206] fix(goal_planner): change debug polygons to contain the nominal margin (#5365) * fixed so that the visualize polygons contain the nominal margin, with some refactoring --------- Signed-off-by: Yuki Takagi Co-authored-by: Kyoichi Sugahara --- .../path_safety_checker/safety_check.hpp | 14 ++---- .../goal_planner/goal_planner_module.cpp | 45 ++++++++++++++----- .../path_safety_checker/safety_check.cpp | 44 +++--------------- 3 files changed, 42 insertions(+), 61 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index 8409116236169..b8bd1629d5f3f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -107,10 +107,6 @@ bool checkCollision( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); -std::vector generatePolygonsWithStoppingAndInertialMargin( - const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, - const double width, const double maximum_deceleration, const double max_extra_stopping_margin); - /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -133,13 +129,9 @@ std::vector getCollidedPolygons( const BehaviorPathPlannerParameters & common_parameters, const RSSparams & rss_parameters, const double hysteresis_factor, CollisionCheckDebug & debug); -/** - * @brief Check collision between ego polygons with margin and objects. - * @return Has collision or not - */ -bool checkCollisionWithMargin( - const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, - const double collision_check_margin); +bool checkPolygonsIntersects( + const std::vector & polys_1, const std::vector & polys_2); + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 95ac47494d016..46b358948863b 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -22,6 +22,7 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -1314,7 +1315,6 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const return true; } } - if (!parameters_->use_object_recognition) { return false; } @@ -1328,20 +1328,41 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const utils::path_safety_checker::isPolygonOverlapLanelet); const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( pull_over_lane_objects, parameters_->th_moving_object_velocity); - const auto common_parameters = planner_data_->parameters; - const double base_link2front = common_parameters.base_link2front; - const double base_link2rear = common_parameters.base_link2rear; - const double vehicle_width = common_parameters.vehicle_width; - - const auto ego_polygons_expanded = - utils::path_safety_checker::generatePolygonsWithStoppingAndInertialMargin( - path, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, + std::vector obj_polygons; + for (const auto & object : pull_over_lane_stop_objects.objects) { + obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object)); + } + + std::vector ego_polygons_expanded; + const auto curvatures = motion_utils::calcCurvature(path.points); + for (size_t i = 0; i < path.points.size(); ++i) { + const auto p = path.points.at(i); + + const double extra_stopping_margin = std::min( + std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_max_extra_stopping_margin); + + double extra_lateral_margin = (-1) * curvatures.at(i) * p.point.longitudinal_velocity_mps * + std::abs(p.point.longitudinal_velocity_mps); + extra_lateral_margin = + std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); + + const auto lateral_offset_pose = + tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); + const auto ego_polygon = tier4_autoware_utils::toFootprint( + lateral_offset_pose, + planner_data_->parameters.base_link2front + + parameters_->object_recognition_collision_check_margin + extra_stopping_margin, + planner_data_->parameters.base_link2rear + + parameters_->object_recognition_collision_check_margin, + planner_data_->parameters.vehicle_width + + parameters_->object_recognition_collision_check_margin * 2.0 + + std::abs(extra_lateral_margin)); + ego_polygons_expanded.push_back(ego_polygon); + } debug_data_.ego_polygons_expanded = ego_polygons_expanded; - return utils::path_safety_checker::checkCollisionWithMargin( - ego_polygons_expanded, pull_over_lane_stop_objects, - parameters_->object_recognition_collision_check_margin); + return utils::path_safety_checker::checkPolygonsIntersects(ego_polygons_expanded, obj_polygons); } bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) const diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index 8ebc144a34584..40aa184105edb 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -18,7 +18,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include +#include #include #include @@ -380,44 +380,12 @@ std::vector getCollidedPolygons( return collided_polygons; } -std::vector generatePolygonsWithStoppingAndInertialMargin( - const PathWithLaneId & ego_path, const double base_to_front, const double base_to_rear, - const double width, const double maximum_deceleration, const double max_extra_stopping_margin) +bool checkPolygonsIntersects( + const std::vector & polys_1, const std::vector & polys_2) { - std::vector polygons; - const auto curvatures = motion_utils::calcCurvature(ego_path.points); - - for (size_t i = 0; i < ego_path.points.size(); ++i) { - const auto p = ego_path.points.at(i); - - const double extra_stopping_margin = std::min( - std::pow(p.point.longitudinal_velocity_mps, 2) * 0.5 / maximum_deceleration, - max_extra_stopping_margin); - - double extra_lateral_margin = (-1) * curvatures[i] * p.point.longitudinal_velocity_mps * - std::abs(p.point.longitudinal_velocity_mps); - extra_lateral_margin = - std::clamp(extra_lateral_margin, -extra_stopping_margin, extra_stopping_margin); - - const auto lateral_offset_pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0.0, extra_lateral_margin / 2.0, 0.0); - const auto ego_polygon = tier4_autoware_utils::toFootprint( - lateral_offset_pose, base_to_front + extra_stopping_margin, base_to_rear, - width + std::abs(extra_lateral_margin)); - polygons.push_back(ego_polygon); - } - return polygons; -} - -bool checkCollisionWithMargin( - const std::vector & ego_polygons, const PredictedObjects & dynamic_objects, - const double collision_check_margin) -{ - for (const auto & ego_polygon : ego_polygons) { - for (const auto & object : dynamic_objects.objects) { - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); - const double distance = boost::geometry::distance(obj_polygon, ego_polygon); - if (distance < collision_check_margin) { + for (const auto & poly_1 : polys_1) { + for (const auto & poly_2 : polys_2) { + if (boost::geometry::intersects(poly_1, poly_2)) { return true; } } From 92f78a476d47bf0811cbb69315fd403be574224f Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 25 Oct 2023 11:39:02 +0900 Subject: [PATCH 166/206] fix(goal_planner): revert "refactor(goal_planner): rebuild state transition (#5371)" (#5399) This reverts commit 3d2a7f6038627ca52a4f0c2af6f5a8d56bc6962b. --- .../goal_planner/goal_planner_module.hpp | 19 +++--- .../goal_planner/goal_planner_module.cpp | 59 +++++++++++-------- 2 files changed, 46 insertions(+), 32 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 21372ed6e2c2f..343302a022a69 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -217,28 +217,31 @@ class GoalPlannerModule : public SceneModuleInterface } } - CandidateOutput planCandidate() const override; - BehaviorModuleOutput plan() override; - BehaviorModuleOutput planWaitingApproval() override; + // TODO(someone): remove this, and use base class function + [[deprecated]] BehaviorModuleOutput run() override; bool isExecutionRequested() const override; bool isExecutionReady() const override; + // TODO(someone): remove this, and use base class function + [[deprecated]] ModuleStatus updateState() override; + BehaviorModuleOutput plan() override; + BehaviorModuleOutput planWaitingApproval() override; + void processOnEntry() override; void processOnExit() override; - void updateData() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { } + // not used, but need to override + CandidateOutput planCandidate() const override { return CandidateOutput{}; }; + private: - // The start_planner activates when it receives a new route, - // so there is no need to terminate the goal planner. - // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return true; } + bool canTransitIdleToRunningState() override { return false; } mutable StartGoalPlannerData goal_planner_data_; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 46b358948863b..87417bfe6a42d 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -114,13 +114,6 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); - } - status_.reset(); } @@ -227,18 +220,16 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -void GoalPlannerModule::updateData() +BehaviorModuleOutput GoalPlannerModule::run() { - // Initialize Occupancy Grid Map - // This operation requires waiting for `planner_data_`, hence it is executed here instead of in - // the constructor. Ideally, this operation should only need to be performed once. - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - initializeOccupancyGridMap(); + current_state_ = ModuleStatus::RUNNING; + updateOccupancyGrid(); + + if (!isActivated()) { + return planWaitingApproval(); } - updateOccupancyGrid(); + return plan(); } void GoalPlannerModule::initializeOccupancyGridMap() @@ -265,6 +256,22 @@ void GoalPlannerModule::initializeSafetyCheckParameters() objects_filtering_params_, parameters_); } +void GoalPlannerModule::processOnEntry() +{ + // Initialize occupancy grid map + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { + initializeOccupancyGridMap(); + } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } +} + void GoalPlannerModule::processOnExit() { resetPathCandidate(); @@ -431,6 +438,13 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } +ModuleStatus GoalPlannerModule::updateState() +{ + // start_planner module will be run when setting new goal, so not need finishing pull_over module. + // Finishing it causes wrong lane_following path generation. + return current_state_; +} + bool GoalPlannerModule::planFreespacePath() { goal_searcher_->setPlannerData(planner_data_); @@ -884,12 +898,6 @@ void GoalPlannerModule::decideVelocity() status_.set_has_decided_velocity(true); } -CandidateOutput GoalPlannerModule::planCandidate() const -{ - return CandidateOutput( - status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); -} - BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { constexpr double path_update_duration = 1.0; @@ -928,7 +936,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(planCandidate().path_candidate); + path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible @@ -981,7 +989,10 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = std::make_shared(planCandidate().path_candidate); + path_candidate_ = + status_.get_is_safe_static_objects() + ? std::make_shared(status_.get_pull_over_path()->getFullPath()) + : out.path; path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); From ce3db689c924f192350c23f3e35ed8f3acc52e5a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 25 Oct 2023 11:39:12 +0900 Subject: [PATCH 167/206] fix(lane_change): fix terminal stop distance (#5392) * fix(lane_change): fix terminal stop distance Signed-off-by: kosuke55 * make current lanes include path front id Signed-off-by: kosuke55 * fixup! make current lanes include path front id --------- Signed-off-by: kosuke55 --- .../behavior_path_planner/src/utils/utils.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 9b10dfc32457f..35dd7675ae3fc 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3081,9 +3081,24 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(reference_lanes, current_pose, ¤t_lane); - - return route_handler->getLaneletSequence( + auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); + + // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids'. + const auto front_lane_ids = path.points.front().lane_ids; + auto have_front_lanes = [front_lane_ids](const auto & lanes) { + return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) { + return std::find(front_lane_ids.begin(), front_lane_ids.end(), lane.id()) != + front_lane_ids.end(); + }); + }; + while (!have_front_lanes(current_lanes)) { + const auto extended_lanes = extendPrevLane(route_handler, current_lanes); + if (extended_lanes.size() == current_lanes.size()) break; + current_lanes = extended_lanes; + } + + return current_lanes; } lanelet::ConstLanelets extendNextLane( From aabbe102ed62f2dd5b8994f171b9bad675b5fc0b Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 25 Oct 2023 11:54:15 +0900 Subject: [PATCH 168/206] refactor(behavior_path_planner): change start planner's variable name (#5390) * change variable name Signed-off-by: kyoichi-sugahara * change variable name to driving_forward Signed-off-by: kyoichi-sugahara * fix typo Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../start_planner/start_planner_module.hpp | 7 +-- .../scene_module/start_planner/manager.cpp | 4 +- .../start_planner/start_planner_module.cpp | 60 +++++++++---------- 3 files changed, 35 insertions(+), 36 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 7ab404d99bb12..8124f8f993906 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -59,10 +59,9 @@ struct PullOutStatus PlannerType planner_type{PlannerType::NONE}; PathWithLaneId backward_path{}; lanelet::ConstLanelets pull_out_lanes{}; - bool is_safe_static_objects{false}; // current path is safe against static objects + bool found_pull_out_path{false}; // current path is safe against static objects bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects - bool back_finished{false}; // if backward driving is not required, this is also set to true - // todo: rename to clear variable name. + bool driving_forward{false}; // if ego is driving on backward path, this is set to false bool backward_driving_complete{ false}; // after backward driving is complete, this is set to true (warning: this is set to // false at next cycle after backward driving is complete) @@ -120,7 +119,7 @@ class StartPlannerModule : public SceneModuleInterface } // Condition to disable simultaneous execution - bool isBackFinished() const { return status_.back_finished; } + bool isDrivingForward() const { return status_.driving_forward; } bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } private: diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 30810bda252c3..46db10417d19e 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -320,7 +320,7 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); // Currently simultaneous execution with other modules is not supported while backward driving - if (!start_planner_ptr->isBackFinished()) { + if (!start_planner_ptr->isDrivingForward()) { return false; } @@ -349,7 +349,7 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); // Currently simultaneous execution with other modules is not supported while backward driving - if (!start_planner_ptr->isBackFinished()) { + if (start_planner_ptr->isDrivingForward()) { return false; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 62f3dbfe191ac..8228542141c77 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -135,8 +135,8 @@ void StartPlannerModule::updateData() if (has_received_new_route) { status_ = PullOutStatus(); } - // check safety status after back finished - if (parameters_->safety_check_params.enable_safety_check && status_.back_finished) { + // check safety status when driving forward + if (parameters_->safety_check_params.enable_safety_check && status_.driving_forward) { status_.is_safe_dynamic_objects = isSafePath(); } else { status_.is_safe_dynamic_objects = true; @@ -184,15 +184,15 @@ bool StartPlannerModule::isExecutionRequested() const bool StartPlannerModule::isExecutionReady() const { - // when is_safe_static_objects is false,the path is not generated and approval shouldn't be + // when found_pull_out_path is false,the path is not generated and approval shouldn't be // allowed - if (!status_.is_safe_static_objects) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against static objects"); + if (!status_.found_pull_out_path) { + RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Pull over path is not found"); return false; } if ( - parameters_->safety_check_params.enable_safety_check && status_.back_finished && + parameters_->safety_check_params.enable_safety_check && status_.driving_forward && isWaitingApproval()) { if (!isSafePath()) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); @@ -250,7 +250,7 @@ BehaviorModuleOutput StartPlannerModule::plan() } BehaviorModuleOutput output; - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); const auto output = generateStopOutput(); @@ -262,7 +262,7 @@ BehaviorModuleOutput StartPlannerModule::plan() PathWithLaneId path; // Check if backward motion is finished - if (status_.back_finished) { + if (status_.driving_forward) { // Increment path index if the current path is finished if (hasFinishedCurrentPath()) { RCLCPP_INFO(getLogger(), "Increment path index"); @@ -317,7 +317,7 @@ BehaviorModuleOutput StartPlannerModule::plan() return SteeringFactor::STRAIGHT; }); - if (status_.back_finished) { + if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -369,7 +369,7 @@ PathWithLaneId StartPlannerModule::getFullPath() const pull_out_path.points.end(), partial_path.points.begin(), partial_path.points.end()); } - if (status_.back_finished) { + if (status_.driving_forward) { // not need backward path or finish it return pull_out_path; } @@ -396,7 +396,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() } BehaviorModuleOutput output; - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull out path, publish stop path"); clearWaitingApproval(); @@ -414,7 +414,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - auto stop_path = status_.back_finished ? getCurrentPath() : status_.backward_path; + auto stop_path = status_.driving_forward ? getCurrentPath() : status_.backward_path; const auto drivable_lanes = generateDrivableLanes(stop_path); const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto expanded_lanes = utils::expandLanelets( @@ -441,7 +441,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() return SteeringFactor::STRAIGHT; }); - if (status_.back_finished) { + if (status_.driving_forward) { const double start_distance = motion_utils::calcSignedArcLength( stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); @@ -502,7 +502,7 @@ void StartPlannerModule::planWithPriority( const auto & pull_out_start_pose = start_pose_candidates.at(i); // Set back_finished to true if the current start pose is same to refined_start_pose - status_.back_finished = + status_.driving_forward = tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; planner->setPlannerData(planner_data_); @@ -512,9 +512,9 @@ void StartPlannerModule::planWithPriority( return false; } // use current path if back is not needed - if (status_.back_finished) { + if (status_.driving_forward) { const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = true; + status_.found_pull_out_path = true; status_.pull_out_path = *pull_out_path; status_.pull_out_start_pose = pull_out_start_pose; status_.planner_type = planner->getPlannerType(); @@ -535,7 +535,7 @@ void StartPlannerModule::planWithPriority( // Update status variables with the next path information { const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = true; + status_.found_pull_out_path = true; status_.pull_out_path = *pull_out_path_next; status_.pull_out_start_pose = pull_out_start_pose_next; status_.planner_type = planner->getPlannerType(); @@ -587,7 +587,7 @@ void StartPlannerModule::planWithPriority( // not found safe path if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); - status_.is_safe_static_objects = false; + status_.found_pull_out_path = false; status_.planner_type = PlannerType::NONE; } } @@ -730,7 +730,7 @@ void StartPlannerModule::updatePullOutStatus() void StartPlannerModule::updateStatusAfterBackwardDriving() { - status_.back_finished = true; + status_.driving_forward = true; status_.backward_driving_complete = true; // request start_planner approval waitApproval(); @@ -835,7 +835,7 @@ bool StartPlannerModule::isOverlappedWithLane( bool StartPlannerModule::hasFinishedPullOut() const { - if (!status_.back_finished || !status_.is_safe_static_objects) { + if (!status_.driving_forward || !status_.found_pull_out_path) { return false; } @@ -874,7 +874,7 @@ bool StartPlannerModule::isBackwardDrivingComplete() const const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; - const bool back_finished = !status_.back_finished && is_near && is_stopped; + const bool back_finished = !status_.driving_forward && is_near && is_stopped; if (back_finished) { RCLCPP_INFO(getLogger(), "back finished"); } @@ -916,7 +916,7 @@ bool StartPlannerModule::isStuck() } // not found safe path - if (!status_.is_safe_static_objects) { + if (!status_.found_pull_out_path) { return true; } @@ -943,7 +943,7 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const Pose & end_pose = status_.pull_out_path.end_pose; // turn on hazard light when backward driving - if (!status_.back_finished) { + if (!status_.driving_forward) { turn_signal.hazard_signal.command = HazardLightsCommand::ENABLE; const auto back_start_pose = planner_data_->route_handler->getOriginalStartPose(); turn_signal.desired_start_point = back_start_pose; @@ -1157,7 +1157,7 @@ BehaviorModuleOutput StartPlannerModule::generateStopOutput() { const std::lock_guard lock(mutex_); - status_.back_finished = true; + status_.driving_forward = true; status_.planner_type = PlannerType::STOP; status_.pull_out_path.partial_paths.clear(); status_.pull_out_path.partial_paths.push_back(stop_path); @@ -1209,8 +1209,8 @@ bool StartPlannerModule::planFreespacePath() status_.pull_out_path = *freespace_path; status_.pull_out_start_pose = current_pose; status_.planner_type = freespace_planner_->getPlannerType(); - status_.is_safe_static_objects = true; - status_.back_finished = true; + status_.found_pull_out_path = true; + status_.driving_forward = true; return true; } @@ -1231,7 +1231,7 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; output.drivable_area_info = - status_.back_finished + status_.driving_forward ? utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) : current_drivable_area_info; @@ -1291,13 +1291,13 @@ void StartPlannerModule::setDebugData() const const auto header = planner_data_->route_handler->getRouteHeader(); { visualization_msgs::msg::MarkerArray planner_type_marker_array{}; - const auto color = status_.is_safe_static_objects ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); + const auto color = status_.found_pull_out_path ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); auto marker = createDefaultMarker( header.frame_id, header.stamp, "planner_type", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); marker.pose = status_.pull_out_start_pose; - if (!status_.back_finished) { + if (!status_.driving_forward) { marker.text = "BACK -> "; } marker.text += magic_enum::enum_name(status_.planner_type); From ae5035f4d4b724a6dde175d4ac9c53ed0f740905 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Wed, 25 Oct 2023 13:24:57 +0900 Subject: [PATCH 169/206] fix(motion_utils): fix build error (#5404) Signed-off-by: Fumiya Watanabe --- .../motion_utils/vehicle/vehicle_state_checker.hpp | 8 ++++---- common/motion_utils/src/vehicle/vehicle_state_checker.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index f5f423f881ba2..6a9bd01a5a9a0 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -55,11 +55,11 @@ class VehicleStopChecker : public VehicleStopCheckerBase protected: rclcpp::Subscription::SharedPtr sub_odom_; - Odometry::SharedPtr odometry_ptr_; + Odometry::ConstSharedPtr odometry_ptr_; private: static constexpr double velocity_buffer_time_sec = 10.0; - void onOdom(const Odometry::SharedPtr msg); + void onOdom(const Odometry::ConstSharedPtr msg); }; class VehicleArrivalChecker : public VehicleStopChecker @@ -74,9 +74,9 @@ class VehicleArrivalChecker : public VehicleStopChecker rclcpp::Subscription::SharedPtr sub_trajectory_; - Trajectory::SharedPtr trajectory_ptr_; + Trajectory::ConstSharedPtr trajectory_ptr_; - void onTrajectory(const Trajectory::SharedPtr msg); + void onTrajectory(const Trajectory::ConstSharedPtr msg); }; } // namespace motion_utils diff --git a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp index 6ed9472d39e0f..194c49ae3bff8 100644 --- a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp +++ b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -88,7 +88,7 @@ VehicleStopChecker::VehicleStopChecker(rclcpp::Node * node) std::bind(&VehicleStopChecker::onOdom, this, _1)); } -void VehicleStopChecker::onOdom(const Odometry::SharedPtr msg) +void VehicleStopChecker::onOdom(const Odometry::ConstSharedPtr msg) { odometry_ptr_ = msg; @@ -128,7 +128,7 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati th_arrived_distance_m; } -void VehicleArrivalChecker::onTrajectory(const Trajectory::SharedPtr msg) +void VehicleArrivalChecker::onTrajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ptr_ = msg; } From 71848084513463e86729693d41b39c2bf9aa3288 Mon Sep 17 00:00:00 2001 From: Shohei Sakai Date: Wed, 25 Oct 2023 13:39:03 +0900 Subject: [PATCH 170/206] feat(map_loader): display curbstone as marker array (#4958) display curbstone as marker array Signed-off-by: Shohei Sakai Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- .../lanelet2_map_visualization_node.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index 53794fb93b7c6..c325f183fd783 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -127,13 +127,14 @@ void Lanelet2MapVisualizationNode::onMapBin( lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); std::vector no_parking_reg_elems = lanelet::utils::query::noParkingAreas(all_lanelets); + lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas; + cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -156,6 +157,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); + setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); visualization_msgs::msg::MarkerArray map_marker_array; @@ -242,6 +244,10 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); + insertMarkerArray( + &map_marker_array, + lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); + pub_marker_->publish(map_marker_array); } From 6e6053625dac33d3d4d12148da43c78519d13bed Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 25 Oct 2023 13:54:53 +0900 Subject: [PATCH 171/206] refactor(behavior_path_planner): refactor planWithPriority in start_planner (#5393) * minor refactor planWithPriority Signed-off-by: kyoichi-sugahara * apply changes based on comments Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara Co-authored-by: Kosuke Takeuchi --- .../start_planner/start_planner_module.hpp | 15 ++ .../start_planner/start_planner_module.cpp | 160 ++++++++++-------- 2 files changed, 100 insertions(+), 75 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 8124f8f993906..206ca5ea080c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -51,6 +51,7 @@ using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; +using PriorityOrder = std::vector>>; struct PullOutStatus { @@ -131,6 +132,20 @@ class StartPlannerModule : public SceneModuleInterface void initializeSafetyCheckParameters(); + PriorityOrder determinePriorityOrder( + const std::string & search_priority, const size_t candidates_size); + bool findPullOutPath( + const std::vector & start_pose_candidates, const size_t index, + const std::shared_ptr & planner, const Pose & refined_start_pose, + const Pose & goal_pose); + void updateStatusWithCurrentPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type); + void updateStatusWithNextPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type); + void updateStatusIfNoSafePathFound(); + std::shared_ptr parameters_; mutable std::shared_ptr ego_predicted_path_params_; mutable std::shared_ptr objects_filtering_params_; diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 8228542141c77..622cc9e3b87ae 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -492,99 +492,109 @@ void StartPlannerModule::planWithPriority( const std::vector & start_pose_candidates, const Pose & refined_start_pose, const Pose & goal_pose, const std::string search_priority) { - // check if start pose candidates are valid - if (start_pose_candidates.empty()) { - return; - } - - const auto is_safe_with_pose_planner = [&](const size_t i, const auto & planner) { - // Get the pull_out_start_pose for the current index - const auto & pull_out_start_pose = start_pose_candidates.at(i); - - // Set back_finished to true if the current start pose is same to refined_start_pose - status_.driving_forward = - tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; - - planner->setPlannerData(planner_data_); - const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); - // not found safe path - if (!pull_out_path) { - return false; - } - // use current path if back is not needed - if (status_.driving_forward) { - const std::lock_guard lock(mutex_); - status_.found_pull_out_path = true; - status_.pull_out_path = *pull_out_path; - status_.pull_out_start_pose = pull_out_start_pose; - status_.planner_type = planner->getPlannerType(); - return true; - } + if (start_pose_candidates.empty()) return; - // If this is the last start pose candidate, return false - if (i == start_pose_candidates.size() - 1) return false; + const PriorityOrder order_priority = + determinePriorityOrder(search_priority, start_pose_candidates.size()); - // check next path if back is needed - const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); - const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); - // not found safe path - if (!pull_out_path_next) { - return false; - } + for (const auto & [index, planner] : order_priority) { + if (findPullOutPath(start_pose_candidates, index, planner, refined_start_pose, goal_pose)) + return; + } - // Update status variables with the next path information - { - const std::lock_guard lock(mutex_); - status_.found_pull_out_path = true; - status_.pull_out_path = *pull_out_path_next; - status_.pull_out_start_pose = pull_out_start_pose_next; - status_.planner_type = planner->getPlannerType(); - } - return true; - }; + updateStatusIfNoSafePathFound(); +} - using PriorityOrder = std::vector>>; - const auto make_loop_order_planner_first = [&]() { - PriorityOrder order_priority; +PriorityOrder StartPlannerModule::determinePriorityOrder( + const std::string & search_priority, const size_t candidates_size) +{ + PriorityOrder order_priority; + if (search_priority == "efficient_path") { for (const auto & planner : start_planners_) { - for (size_t i = 0; i < start_pose_candidates.size(); i++) { + for (size_t i = 0; i < candidates_size; i++) { order_priority.emplace_back(i, planner); } } - return order_priority; - }; - - const auto make_loop_order_pose_first = [&]() { - PriorityOrder order_priority; - for (size_t i = 0; i < start_pose_candidates.size(); i++) { + } else if (search_priority == "short_back_distance") { + for (size_t i = 0; i < candidates_size; i++) { for (const auto & planner : start_planners_) { order_priority.emplace_back(i, planner); } } - return order_priority; - }; - - // Choose loop order based on priority_on_efficient_path - PriorityOrder order_priority; - if (search_priority == "efficient_path") { - order_priority = make_loop_order_planner_first(); - } else if (search_priority == "short_back_distance") { - order_priority = make_loop_order_pose_first(); } else { - RCLCPP_ERROR( - getLogger(), - "search_priority should be efficient_path or short_back_distance, but %s is given.", - search_priority.c_str()); + RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str()); throw std::domain_error("[start_planner] invalid search_priority"); } + return order_priority; +} - for (const auto & p : order_priority) { - if (is_safe_with_pose_planner(p.first, p.second)) { - return; - } +bool StartPlannerModule::findPullOutPath( + const std::vector & start_pose_candidates, const size_t index, + const std::shared_ptr & planner, const Pose & refined_start_pose, + const Pose & goal_pose) +{ + // Ensure the index is within the bounds of the start_pose_candidates vector + if (index >= start_pose_candidates.size()) return false; + + const Pose & pull_out_start_pose = start_pose_candidates.at(index); + const bool is_driving_forward = + tier4_autoware_utils::calcDistance2d(pull_out_start_pose, refined_start_pose) < 0.01; + + planner->setPlannerData(planner_data_); + const auto pull_out_path = planner->plan(pull_out_start_pose, goal_pose); + + // If no path is found, return false + if (!pull_out_path) { + return false; } - // not found safe path + // If driving forward, update status with the current path and return true + if (is_driving_forward) { + updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner->getPlannerType()); + return true; + } + + // If this is the last start pose candidate, return false + if (index == start_pose_candidates.size() - 1) return false; + + const Pose & next_pull_out_start_pose = start_pose_candidates.at(index + 1); + const auto next_pull_out_path = planner->plan(next_pull_out_start_pose, goal_pose); + + // If no next path is found, return false + if (!next_pull_out_path) return false; + + // Update status with the next path and return true + updateStatusWithNextPath( + *next_pull_out_path, next_pull_out_start_pose, planner->getPlannerType()); + return true; +} + +void StartPlannerModule::updateStatusWithCurrentPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type) +{ + const std::lock_guard lock(mutex_); + status_.driving_forward = true; + status_.found_pull_out_path = true; + status_.pull_out_path = path; + status_.pull_out_start_pose = start_pose; + status_.planner_type = planner_type; +} + +void StartPlannerModule::updateStatusWithNextPath( + const behavior_path_planner::PullOutPath & path, const Pose & start_pose, + const behavior_path_planner::PlannerType & planner_type) +{ + const std::lock_guard lock(mutex_); + status_.driving_forward = false; + status_.found_pull_out_path = true; + status_.pull_out_path = path; + status_.pull_out_start_pose = start_pose; + status_.planner_type = planner_type; +} + +void StartPlannerModule::updateStatusIfNoSafePathFound() +{ if (status_.planner_type != PlannerType::FREESPACE) { const std::lock_guard lock(mutex_); status_.found_pull_out_path = false; From d6be0d2089c76f5530aef46f0361afaa21b54548 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 25 Oct 2023 17:15:40 +0900 Subject: [PATCH 172/206] feat(avoidance): add use_conservative_buffer_longitudinal (#5405) Signed-off-by: Takayuki Murooka --- .../config/avoidance/avoidance.param.yaml | 8 +++++ .../utils/avoidance/avoidance_module_data.hpp | 2 ++ .../avoidance/avoidance_module.cpp | 34 ++++++++++++++----- .../src/scene_module/avoidance/manager.cpp | 5 +++ 4 files changed, 40 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 25e42d20d144a..3d0a334cedf84 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -37,6 +37,7 @@ avoid_margin_lateral: 1.0 # [m] safety_buffer_lateral: 0.7 # [m] safety_buffer_longitudinal: 0.0 # [m] + use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: is_target: true execute_num: 1 @@ -47,6 +48,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bus: is_target: true execute_num: 1 @@ -57,6 +59,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true trailer: is_target: true execute_num: 1 @@ -67,6 +70,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true unknown: is_target: false execute_num: 1 @@ -77,6 +81,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 + use_conservative_buffer_longitudinal: true bicycle: is_target: false execute_num: 1 @@ -87,6 +92,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true motorcycle: is_target: false execute_num: 1 @@ -97,6 +103,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true pedestrian: is_target: false execute_num: 1 @@ -107,6 +114,7 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 + use_conservative_buffer_longitudinal: true lower_distance_for_polygon_expansion: 30.0 # [m] upper_distance_for_polygon_expansion: 100.0 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 74464be0fa5ea..e881c8a0d3b18 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -71,6 +71,8 @@ struct ObjectParameter double safety_buffer_lateral{1.0}; double safety_buffer_longitudinal{0.0}; + + bool use_conservative_buffer_longitudinal{true}; }; struct AvoidanceParameters diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 84064a1c38bcb..5423cff11bfeb 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -631,7 +631,10 @@ void AvoidanceModule::fillDebugData( const auto o_front = data.stop_target_object.get(); const auto object_type = utils::getHighestProbLabel(o_front.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & base_link2front = planner_data_->parameters.base_link2front; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + @@ -640,7 +643,8 @@ void AvoidanceModule::fillDebugData( const auto variable = helper_.getSharpAvoidanceDistance( helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); const auto constant = helper_.getNominalPrepareDistance() + - object_parameter.safety_buffer_longitudinal + base_link2front; + object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal; const auto total_avoid_distance = variable + constant; dead_pose_ = calcLongitudinalOffsetPose( @@ -894,7 +898,6 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( { // To be consistent with changes in the ego position, the current shift length is considered. const auto current_ego_shift = helper_.getEgoShift(); - const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & base_link2rear = planner_data_->parameters.base_link2rear; // Calculate feasible shift length @@ -932,8 +935,12 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( // calculate remaining distance. const auto prepare_distance = helper_.getNominalPrepareDistance(); - const auto constant = - object_parameter.safety_buffer_longitudinal + base_link2front + prepare_distance; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; + const auto constant = object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal + prepare_distance; const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; const auto remaining_distance = object.longitudinal - constant; @@ -1053,7 +1060,12 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidLine al_avoid; { - const auto offset = object_parameter.safety_buffer_longitudinal + base_link2front; + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; + const auto offset = + object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; const auto path_front_to_ego = avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); @@ -2807,7 +2819,6 @@ void AvoidanceModule::updateAvoidanceDebugData( double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const { const auto & p = parameters_; - const auto & base_link2front = planner_data_->parameters.base_link2front; const auto & vehicle_width = planner_data_->parameters.vehicle_width; // D5 @@ -2823,7 +2834,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const // D2: min_avoid_distance // D3: longitudinal_avoid_margin_front (margin + D5) // D4: o_front.longitudinal - // D5: base_link2front + // D5: additional_buffer_longitudinal (base_link2front or 0 depending on the + // use_conservative_buffer_longitudinal) const auto object_type = utils::getHighestProbLabel(object.object.classification); const auto object_parameter = parameters_->object_parameters.at(object_type); @@ -2832,8 +2844,12 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto variable = helper_.getMinAvoidanceDistance( helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal + - base_link2front + p->stop_buffer; + additional_buffer_longitudinal + p->stop_buffer; return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 763cf17890721..7988239e68071 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -82,6 +82,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "safety_buffer_lateral"); param.safety_buffer_longitudinal = getOrDeclareParameter(*node, ns + "safety_buffer_longitudinal"); + param.use_conservative_buffer_longitudinal = + getOrDeclareParameter(*node, ns + "use_conservative_buffer_longitudinal"); return param; }; @@ -336,6 +338,9 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral); updateParam( parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal); + updateParam( + parameters, ns + "use_conservative_buffer_longitudinal", + config.use_conservative_buffer_longitudinal); }; { From 5a76057f094782538de1191b388a8e3fff3323b9 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Wed, 25 Oct 2023 17:51:11 +0900 Subject: [PATCH 173/206] fix(behavior_path_planner): add guard to extend lane (#5406) Signed-off-by: kosuke55 --- planning/behavior_path_planner/src/utils/utils.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 35dd7675ae3fc..6e6198e30b13c 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3104,6 +3104,8 @@ lanelet::ConstLanelets getCurrentLanesFromPath( lanelet::ConstLanelets extendNextLane( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) { + if (lanes.empty()) return lanes; + auto extended_lanes = lanes; // Add next lane @@ -3125,6 +3127,8 @@ lanelet::ConstLanelets extendNextLane( lanelet::ConstLanelets extendPrevLane( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) { + if (lanes.empty()) return lanes; + auto extended_lanes = lanes; // Add previous lane From 4eb0d459a8cf3d37efe14a79f0421872a73c19ee Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 25 Oct 2023 18:45:28 +0900 Subject: [PATCH 174/206] feat(map_loader): show intersection areas (#5401) Signed-off-by: satoshi-ota --- .../lanelet2_map_visualization_node.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index c325f183fd783..082dc95d6500a 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -125,6 +125,8 @@ void Lanelet2MapVisualizationNode::onMapBin( viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); lanelet::ConstPolygons3d hatched_road_markings_area = lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); + lanelet::ConstPolygons3d intersection_areas = + lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "intersection_area"); std::vector no_parking_reg_elems = lanelet::utils::query::noParkingAreas(all_lanelets); lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); @@ -134,7 +136,7 @@ void Lanelet2MapVisualizationNode::onMapBin( cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones; + cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones, cl_intersection_area; setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); @@ -158,6 +160,7 @@ void Lanelet2MapVisualizationNode::onMapBin( setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); + setColor(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); visualization_msgs::msg::MarkerArray map_marker_array; @@ -248,6 +251,10 @@ void Lanelet2MapVisualizationNode::onMapBin( &map_marker_array, lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); + insertMarkerArray( + &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( + intersection_areas, cl_intersection_area)); + pub_marker_->publish(map_marker_array); } From 82a6c0c5d0e3a7a3803f92c9a1334047b80344c5 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 25 Oct 2023 20:27:30 +0900 Subject: [PATCH 175/206] fix(intersection): yield stuck stop (#5403) Signed-off-by: Takayuki Murooka --- .../src/scene_intersection.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 53a99a1ee6b4d..31b63413a6437 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1064,16 +1064,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool yield_stuck_detected = checkYieldStuckVehicle( planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); if (yield_stuck_detected && stuck_stop_line_idx_opt) { - auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { - if ( - default_stop_line_idx_opt && - fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { - stuck_stop_line_idx = default_stop_line_idx_opt.value(); - } - } else { - return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; - } + const auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; } // if attention area is empty, collision/occlusion detection is impossible From 801fd0dc48f7efbda6b92faffeabe281720b260b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Oct 2023 12:18:10 +0900 Subject: [PATCH 176/206] fix(goal_planner): fix hasEnoughDistance (#5411) Signed-off-by: kosuke55 --- .../src/scene_module/goal_planner/goal_planner_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 87417bfe6a42d..241a820175ffe 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -1386,7 +1386,7 @@ bool GoalPlannerModule::hasEnoughDistance(const PullOverPath & pull_over_path) c // so need enough distance to restart. // distance to restart should be less than decide_path_distance. // otherwise, the goal would change immediately after departure. - const bool is_separated_path = status_.get_pull_over_path()->partial_paths.size() > 1; + const bool is_separated_path = pull_over_path.partial_paths.size() > 1; const double distance_to_start = calcSignedArcLength( pull_over_path.getFullPath().points, current_pose.position, pull_over_path.start_pose.position); const double distance_to_restart = parameters_->decide_path_distance / 2; From 98fe02b9d6cd618a002d9dbbf2e6707d2f95498f Mon Sep 17 00:00:00 2001 From: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Date: Thu, 26 Oct 2023 15:13:54 +0900 Subject: [PATCH 177/206] feat(radar_tracks_msgs_converter): add an error message and prevent the node from crashing. (#5400) * add an error message and prevent the node from crashing. Signed-off-by: Shunsuke Miura * style(pre-commit): autofix --------- Signed-off-by: Shunsuke Miura Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../radar_tracks_msgs_converter_node.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 1122bf7b4b18e..d8b5599cc1ca7 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -244,6 +244,11 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() std::atan2(compensated_velocity.y, compensated_velocity.x)); geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; + if (transform_ == nullptr) { + RCLCPP_ERROR_THROTTLE( + get_logger(), *get_clock(), 5000, "getTransform failed. radar output will be empty."); + return tracked_objects; + } tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; kinematics.pose_with_covariance.pose.orientation = From fdd671f149d967b9cb224d992d2979dd5617d217 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Oct 2023 18:05:17 +0900 Subject: [PATCH 178/206] fix(behavior_path_planner): fix lane extension in getCurrentLanesFromPath (#5415) --- .../behavior_path_planner/utils/utils.hpp | 6 ++- .../behavior_path_planner/src/utils/utils.cpp | 40 ++++++++++++++----- 2 files changed, 33 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index f418c9c7300f7..326a6aded5e88 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -365,10 +365,12 @@ lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); lanelet::ConstLanelets extendNextLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); lanelet::ConstLanelets extendPrevLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route = false); lanelet::ConstLanelets extendLanes( const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 6e6198e30b13c..9737db21a31a5 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3084,7 +3084,8 @@ lanelet::ConstLanelets getCurrentLanesFromPath( auto current_lanes = route_handler->getLaneletSequence( current_lane, current_pose, p.backward_path_length, p.forward_path_length); - // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids'. + // Extend the 'current_lanes' with previous lanes until it contains 'front_lane_ids' + // if the extended prior lanes is in same lane sequence with current lanes const auto front_lane_ids = path.points.front().lane_ids; auto have_front_lanes = [front_lane_ids](const auto & lanes) { return std::any_of(lanes.begin(), lanes.end(), [&](const auto & lane) { @@ -3092,17 +3093,23 @@ lanelet::ConstLanelets getCurrentLanesFromPath( front_lane_ids.end(); }); }; - while (!have_front_lanes(current_lanes)) { - const auto extended_lanes = extendPrevLane(route_handler, current_lanes); - if (extended_lanes.size() == current_lanes.size()) break; - current_lanes = extended_lanes; + auto extended_lanes = current_lanes; + while (rclcpp::ok()) { + const size_t pre_extension_size = extended_lanes.size(); // Get existing size before extension + extended_lanes = extendPrevLane(route_handler, extended_lanes, true); + if (extended_lanes.size() == pre_extension_size) break; + if (have_front_lanes(extended_lanes)) { + current_lanes = extended_lanes; + break; + } } return current_lanes; } lanelet::ConstLanelets extendNextLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) { if (lanes.empty()) return lanes; @@ -3111,21 +3118,27 @@ lanelet::ConstLanelets extendNextLane( // Add next lane const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); if (!next_lanes.empty()) { + boost::optional target_next_lane; + if (!only_in_route) { + target_next_lane = next_lanes.front(); + } // use the next lane in route if it exists - auto target_next_lane = next_lanes.front(); for (const auto & next_lane : next_lanes) { if (route_handler->isRouteLanelet(next_lane)) { target_next_lane = next_lane; } } - extended_lanes.push_back(target_next_lane); + if (target_next_lane) { + extended_lanes.push_back(*target_next_lane); + } } return extended_lanes; } lanelet::ConstLanelets extendPrevLane( - const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes) + const std::shared_ptr route_handler, const lanelet::ConstLanelets & lanes, + const bool only_in_route) { if (lanes.empty()) return lanes; @@ -3134,14 +3147,19 @@ lanelet::ConstLanelets extendPrevLane( // Add previous lane const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); if (!prev_lanes.empty()) { + boost::optional target_prev_lane; + if (!only_in_route) { + target_prev_lane = prev_lanes.front(); + } // use the previous lane in route if it exists - auto target_prev_lane = prev_lanes.front(); for (const auto & prev_lane : prev_lanes) { if (route_handler->isRouteLanelet(prev_lane)) { target_prev_lane = prev_lane; } } - extended_lanes.insert(extended_lanes.begin(), target_prev_lane); + if (target_prev_lane) { + extended_lanes.insert(extended_lanes.begin(), *target_prev_lane); + } } return extended_lanes; } From 45792dfee3a930d8a0562abc10788404f94c9a67 Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Thu, 26 Oct 2023 18:32:20 +0900 Subject: [PATCH 179/206] feat(AEB): implement parameterized prediction time horizon and interval (#5413) * feat(AEB): implement parameterized prediction time horizon and interval Signed-off-by: 1222-takeshi * chore: update readme Signed-off-by: 1222-takeshi * style(pre-commit): autofix --------- Signed-off-by: 1222-takeshi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../autonomous_emergency_braking/README.md | 35 ++++++++++--------- .../autonomous_emergency_braking.param.yaml | 7 ++-- .../autonomous_emergency_braking/node.hpp | 7 ++-- .../autonomous_emergency_braking/src/node.cpp | 20 +++++++---- 4 files changed, 43 insertions(+), 26 deletions(-) diff --git a/control/autonomous_emergency_braking/README.md b/control/autonomous_emergency_braking/README.md index d1cd80625f2ac..4eff1583d5df8 100644 --- a/control/autonomous_emergency_braking/README.md +++ b/control/autonomous_emergency_braking/README.md @@ -26,22 +26,25 @@ This module has following assumptions. ## Parameters -| Name | Unit | Type | Description | Default value | -| :------------------------ | :----- | :----- | :-------------------------------------------------------------------------- | :------------ | -| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | -| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | -| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | -| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | -| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | -| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | -| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | -| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | -| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | -| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | -| prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | -| prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | -| aeb_hz | [-] | double | 　frequency at which AEB operates per second | 10 | +| Name | Unit | Type | Description | Default value | +| :--------------------------- | :----- | :----- | :-------------------------------------------------------------------------- | :------------ | +| publish_debug_pointcloud | [-] | bool | flag to publish the point cloud used for debugging | false | +| use_predicted_trajectory | [-] | bool | flag to use the predicted path from the control module | true | +| use_imu_path | [-] | bool | flag to use the predicted path generated by sensor data | true | +| voxel_grid_x | [m] | double | down sampling parameters of x-axis for voxel grid filter | 0.05 | +| voxel_grid_y | [m] | double | down sampling parameters of y-axis for voxel grid filter | 0.05 | +| voxel_grid_z | [m] | double | down sampling parameters of z-axis for voxel grid filter | 100000.0 | +| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| expand_width | [m] | double | expansion width of the ego vehicle for the collision check | 0.1 | +| longitudinal_offset | [m] | double | longitudinal offset distance for collision check | 2.0 | +| t_response | [s] | double | response time for the ego to detect the front vehicle starting deceleration | 1.0 | +| a_ego_min | [m/ss] | double | maximum deceleration value of the ego vehicle | -3.0 | +| a_obj_min | [m/ss] | double | maximum deceleration value of objects | -3.0 | +| imu_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by sensors | 1.5 | +| imu_prediction_time_interval | [s] | double | time interval of the predicted path generated by sensors | 0.1 | +| mpc_prediction_time_horizon | [s] | double | time horizon of the predicted path generated by mpc | 1.5 | +| mpc_prediction_time_interval | [s] | double | time interval of the predicted path generated by mpc | 0.1 | +| aeb_hz | [-] | double | frequency at which AEB operates per second | 10 | ## Inner-workings / Algorithms diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 09db0feb34597..1a870aff7a843 100644 --- a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + publish_debug_pointcloud: false use_predicted_trajectory: true use_imu_path: true voxel_grid_x: 0.05 @@ -11,7 +12,9 @@ t_response: 1.0 a_ego_min: -3.0 a_obj_min: -1.0 - prediction_time_horizon: 1.5 - prediction_time_interval: 0.1 + imu_prediction_time_horizon: 1.5 + imu_prediction_time_interval: 0.1 + mpc_prediction_time_horizon: 1.5 + mpc_prediction_time_interval: 0.1 collision_keeping_sec: 0.0 aeb_hz: 10.0 diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index a8f260795d485..27ec775110109 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -172,6 +172,7 @@ class AEB : public rclcpp::Node Updater updater_{this}; // member variables + bool publish_debug_pointcloud_; bool use_predicted_trajectory_; bool use_imu_path_; double voxel_grid_x_; @@ -183,8 +184,10 @@ class AEB : public rclcpp::Node double t_response_; double a_ego_min_; double a_obj_min_; - double prediction_time_horizon_; - double prediction_time_interval_; + double imu_prediction_time_horizon_; + double imu_prediction_time_interval_; + double mpc_prediction_time_horizon_; + double mpc_prediction_time_interval_; CollisionDataKeeper collision_data_keeper_; }; } // namespace autoware::motion::control::autonomous_emergency_braking diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 0ccc9bc4a3dae..e88a5ee833612 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -125,6 +125,7 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) updater_.add("aeb_emergency_stop", this, &AEB::onCheckCollision); // parameter + publish_debug_pointcloud_ = declare_parameter("publish_debug_pointcloud"); use_predicted_trajectory_ = declare_parameter("use_predicted_trajectory"); use_imu_path_ = declare_parameter("use_imu_path"); voxel_grid_x_ = declare_parameter("voxel_grid_x"); @@ -136,8 +137,10 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) t_response_ = declare_parameter("t_response"); a_ego_min_ = declare_parameter("a_ego_min"); a_obj_min_ = declare_parameter("a_obj_min"); - prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); - prediction_time_interval_ = declare_parameter("prediction_time_interval"); + imu_prediction_time_horizon_ = declare_parameter("imu_prediction_time_horizon"); + imu_prediction_time_interval_ = declare_parameter("imu_prediction_time_interval"); + mpc_prediction_time_horizon_ = declare_parameter("mpc_prediction_time_horizon"); + mpc_prediction_time_interval_ = declare_parameter("mpc_prediction_time_interval"); const auto collision_keeping_sec = declare_parameter("collision_keeping_sec"); collision_data_keeper_.setTimeout(collision_keeping_sec); @@ -227,7 +230,9 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) obstacle_ros_pointcloud_ptr_ = std::make_shared(); pcl::toROSMsg(*no_height_filtered_pointcloud_ptr, *obstacle_ros_pointcloud_ptr_); obstacle_ros_pointcloud_ptr_->header = input_msg->header; - pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); + if (publish_debug_pointcloud_) { + pub_obstacle_pointcloud_->publish(*obstacle_ros_pointcloud_ptr_); + } } bool AEB::isDataReady() @@ -392,8 +397,8 @@ void AEB::generateEgoPath( } constexpr double epsilon = 1e-6; - const double & dt = prediction_time_interval_; - const double & horizon = prediction_time_horizon_; + const double & dt = imu_prediction_time_interval_; + const double & horizon = imu_prediction_time_horizon_; for (double t = 0.0; t < horizon + epsilon; t += dt) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; @@ -452,8 +457,11 @@ void AEB::generateEgoPath( geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped); path.at(i) = map_pose; - } + if (i * mpc_prediction_time_interval_ > mpc_prediction_time_horizon_) { + break; + } + } // create polygon polygons.resize(path.size()); for (size_t i = 0; i < path.size() - 1; ++i) { From 64c8ac88d666204d4a13dfb69c4213a8852a3679 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 26 Oct 2023 19:44:20 +0900 Subject: [PATCH 180/206] refactor(landmark_based_localizer): refactored landmark_tf_caster (#5414) * Removed landmark_tf_caster node Signed-off-by: Shintaro SAKODA * Added maintainer Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Renamed to landmark_parser Signed-off-by: Shintaro SAKODA * Added include Signed-off-by: Shintaro SAKODA * style(pre-commit): autofix * Added publish_landmark_markers Signed-off-by: Shintaro SAKODA * Removed unused package.xml Signed-off-by: Shintaro Sakoda * Changed from depend to build_depend Signed-off-by: Shintaro Sakoda * Fixed a local variable name Signed-off-by: Shintaro Sakoda * Fixed Marker to MarkerArray Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro SAKODA Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ar_tag_based_localizer.launch.xml | 8 +- launch/tier4_localization_launch/package.xml | 1 - .../landmark_based_localizer/README.md | 22 +-- .../ar_tag_based_localizer/README.md | 12 +- .../ar_tag_based_localizer_core.hpp | 7 + .../launch/ar_tag_based_localizer.launch.xml | 2 + .../ar_tag_based_localizer/package.xml | 4 + .../src/ar_tag_based_localizer_core.cpp | 37 ++++- .../doc_image/node_diagram.drawio.svg | 96 +++++------ .../CMakeLists.txt | 9 +- .../landmark_parser/landmark_parser_core.hpp | 34 ++++ .../package.xml | 12 +- .../src/landmark_parser_core.cpp | 154 ++++++++++++++++++ .../config/landmark_tf_caster.param.yaml | 4 - .../landmark_tf_caster_core.hpp | 49 ------ .../launch/landmark_tf_caster.launch.xml | 12 -- .../src/landmark_tf_caster_core.cpp | 126 -------------- .../src/landmark_tf_caster_node.cpp | 22 --- 18 files changed, 296 insertions(+), 315 deletions(-) rename localization/landmark_based_localizer/{landmark_tf_caster => landmark_parser}/CMakeLists.txt (69%) create mode 100644 localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp rename localization/landmark_based_localizer/{landmark_tf_caster => landmark_parser}/package.xml (69%) create mode 100644 localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp delete mode 100644 localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml delete mode 100644 localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp delete mode 100644 localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml delete mode 100644 localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp delete mode 100644 localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index a767d2d253cff..181f470a00800 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -4,6 +4,7 @@ + @@ -12,12 +13,5 @@ - - - - - - - diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 6b3b3d3bbbe01..f00752d1c14fc 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -21,7 +21,6 @@ eagleye_rt ekf_localizer gyro_odometer - landmark_tf_caster ndt_scan_matcher pointcloud_preprocessor pose_initializer diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index a8ebdc0c8f1e5..35d8d78e7015c 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -23,11 +23,11 @@ This calculated ego pose is passed to the EKF, where it is fused with the twist ![node diagram](./doc_image/node_diagram.drawio.svg) -### `landmark_tf_caster` node +### `landmark_parser` The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`. -The `landmark_tf_caster` node publishes the TF from the map to the landmark. +The `landmark_parser` is a utility package to load landmarks from the map. - Translation : The center of the four vertices of the landmark - Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. @@ -41,25 +41,9 @@ So, if the 4 vertices are considered as forming a tetrahedron and its volume exc - ar_tag_based_localizer - etc. -## Inputs / Outputs - -### `landmark_tf_caster` node - -#### Input - -| Name | Type | Description | -| :--------------------- | :------------------------------------------- | :--------------- | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | - -#### Output - -| Name | Type | Description | -| :---------- | :------------------------------------- | :----------------- | -| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | - ## Map specifications -For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_tf_caster` can interpret. +For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_parser` can interpret. The four vertices of a landmark are defined counterclockwise. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index b212711f38a57..f0c54d6393f7e 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -13,11 +13,12 @@ The positions and orientations of the AR-Tags are assumed to be written in the L #### Input -| Name | Type | Description | -| :-------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | -| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | -| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | +| Name | Type | Description | +| :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | #### Output @@ -25,6 +26,7 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | | `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | | `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | +| `~/debug/marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] Loaded landmarks to visualize in Rviz as thin boards | | `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | | `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp index 37725dd06c34e..facd02e2b9b7b 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -45,6 +45,8 @@ #ifndef AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ #define AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ +#include "landmark_parser/landmark_parser_core.hpp" + #include #include @@ -57,6 +59,7 @@ #include #include +#include #include #include #include @@ -68,6 +71,7 @@ class ArTagBasedLocalizer : public rclcpp::Node bool setup(); private: + void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); @@ -92,11 +96,13 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr it_; // Subscribers + rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers + rclcpp::Publisher::SharedPtr marker_pub_; image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; @@ -106,6 +112,7 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::CameraParameters cam_param_; bool cam_info_received_; geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{}; + std::map landmark_map_; }; #endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index f6c10050b1826..6437455875cc2 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -3,6 +3,7 @@ + @@ -11,6 +12,7 @@ + diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 216fa21bc951a..c0d1b00e35d3b 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -6,6 +6,8 @@ The ar_tag_based_localizer package Shintaro Sakoda Masahiro Sakamoto + Yamato Ando + Kento Yabuuchi Apache License 2.0 ament_cmake @@ -15,6 +17,8 @@ cv_bridge diagnostic_msgs image_transport + landmark_parser + lanelet2_extension rclcpp tf2_eigen tf2_geometry_msgs diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp index afa82ab3e0677..0492e07875a17 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -110,6 +110,9 @@ bool ArTagBasedLocalizer::setup() /* Subscribers */ + map_bin_sub_ = this->create_subscription( + "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), + std::bind(&ArTagBasedLocalizer::map_bin_callback, this, std::placeholders::_1)); rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); image_sub_ = this->create_subscription( @@ -125,6 +128,11 @@ bool ArTagBasedLocalizer::setup() /* Publishers */ + rclcpp::QoS qos_marker = rclcpp::QoS(rclcpp::KeepLast(10)); + qos_marker.transient_local(); + qos_marker.reliable(); + marker_pub_ = + this->create_publisher("~/debug/marker", qos_marker); rclcpp::QoS qos_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); image_pub_ = it_->advertise("~/debug/result", 1); pose_pub_ = this->create_publisher( @@ -136,6 +144,15 @@ bool ArTagBasedLocalizer::setup() return true; } +void ArTagBasedLocalizer::map_bin_callback( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) +{ + landmark_map_ = parse_landmark(msg, "apriltag_16h5", this->get_logger()); + const visualization_msgs::msg::MarkerArray marker_msg = + convert_to_marker_array_msg(landmark_map_); + marker_pub_->publish(marker_msg); +} + void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg) { if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { @@ -290,16 +307,20 @@ void ArTagBasedLocalizer::publish_pose_as_base_link( } // Transform map to tag - geometry_msgs::msg::TransformStamped map_to_tag_tf; - try { - map_to_tag_tf = - tf_buffer_->lookupTransform("map", "tag_" + msg.header.frame_id, tf2::TimePointZero); - } catch (tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Could not transform map to tag_%s: %s", msg.header.frame_id.c_str(), - ex.what()); + if (landmark_map_.count(msg.header.frame_id) == 0) { + RCLCPP_INFO_STREAM( + this->get_logger(), "frame_id(" << msg.header.frame_id << ") is not in landmark_map_"); return; } + const geometry_msgs::msg::Pose & tag_pose = landmark_map_.at(msg.header.frame_id); + geometry_msgs::msg::TransformStamped map_to_tag_tf; + map_to_tag_tf.header.stamp = msg.header.stamp; + map_to_tag_tf.header.frame_id = "map"; + map_to_tag_tf.child_frame_id = msg.header.frame_id; + map_to_tag_tf.transform.translation.x = tag_pose.position.x; + map_to_tag_tf.transform.translation.y = tag_pose.position.y; + map_to_tag_tf.transform.translation.z = tag_pose.position.z; + map_to_tag_tf.transform.rotation = tag_pose.orientation; // Transform camera to base_link geometry_msgs::msg::TransformStamped camera_to_base_link_tf; diff --git a/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg index 120a1f2aa3391..e592f06a5a86c 100644 --- a/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg @@ -3,90 +3,84 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="721px" + width="568px" height="331px" - viewBox="-0.5 -0.5 721 331" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1VnLjpswFP2aLBsFG0iybF7TxVQdaSq1XSEHDFgDGBnn1a+vHQwE8ExIQiYqm+BjA/Y51/fhDOA83j8xlIbfqYejARh5+wFcDAAwgA3Ej0QOOWIDMwcCRjw1qAJeyV+swJFCN8TDWW0gpzTiJK2DLk0S7PIahhiju/own0b1r6YowC3g1UVRG/1FPB7m6ASMK/wbJkFYfNmwp3lPjIrBaiVZiDy6O4HgcgDnjFKe38X7OY4keQUv+XOrd3rLiTGc8C4PKCG2KNqotal58UOxWEY3iYfl+NEAznyacCWIAVR7TiPKjoPh6HgJvD0RNbctZhzvTyA1sSdMY8zZQQxRvcBSJCkrgaq5qygvsfCEblthSKkclG+uiBA3igs9L2aLl2eUeDFibwKdoUzwAWyxXkOu9ZkK0xCcsHPckSg64Wo+X1orMZ9Zxhl9wyc9tjvBa/++7MJxnV2zza4x0rBr9sCu1WL3Bw8lfaOvG053Ylkn7KbIfRMbKruU3NVqPteROwFraNt3Nl37ceTCsWZPr/iOZNzZER46Lt0iRlDi4hal2BN+TjUp4yENaIKiZYXOBCfs8Ftxfmz8kY3h2Crai/1p7+KgWu+yyhELsIKgwuQ8PuSZ4Qhxsq37Yx1r6tEXSsRnK+Nv6APNSf0VGd0wF6unGtyX0+gkR+Hp63KQWIaYj+hPaPIR36AXvo3x5/BtQqvGNxg3DD2fVIvv1ovAqC4cgPBuwk10wqU0wxdto0LHPeFHGYeWakkdDXVfaSgbh1NBe9U+Z6eu/VX7T8nxZTQ0xJU/dKuNjOs2AuGlNlIMpL6f4VvlN7RuFDGHo8BZywzAiW6M++8FoHY+cLz6CU1mcwcBTWyyNLHJ6iM2GTpS8Zt/PZcqzD+ES7uZoRqfyaXWP7koxgw5JPHpLeGln3D+WeGlFRWsbuFcE6fqLzImVicfdIV/MaFGvvOCdXLtcNrWwuw9tepsqNPWSkUxhSPM2ytGWZoX7D7Zyz0/SzEj4otY7l7xWlHh45cKmgmfESSiyxV0HIGyoJZG6qEsLD1HJgoJkgQ/aSoAKIA8Byt/FyQOxFIispZ+3pUUOB5hYjpUrnflIY6k38+G2Tbox39MGtYGZHbQdCA6/2FOb5fFbFdhsk7wnYwL/d0uxljya3S1zKLAua+X6EyBNjePVKXvCC5clPH/q65vxnfDmGqM6m5RyWyfnPTm1kxNiCkkfIDxaHOZSHk2J0apSGqQd3VG85iDi6b1aF3S/azH1nEq88OL664zxxdGPd85l+7oy7eyYtOWb91C9QPTpuYRILw2bbKbxynWeNhX4iSa1XF4Prz6UwEu/wE=</diagram></mxfile>" + viewBox="-0.5 -0.5 568 331" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">xZhLj5swEMc/TY6NMOaRHJvX9rBVV2qltqfIAQesAEbGefXT1w4GAng3JCGbHHbxH2Ps34xnxgzgND68MJSG36mPo4Fp+IcBnA1M0wVA/JXCMRegO8qFgBE/l0Al/CT/sBINpW6Jj7NaR05pxElaFz2aJNjjNQ0xRvf1bmsa1d+aogC3hJ8eitrqb+LzMFdHplvp3zAJwuLNwBnnd2JUdFYryULk0/2ZBOcDOGWU8vwqPkxxJNkVXPLnFu/cLSfGcMK7PGDmD+xQtFVrU/Pix2KxjG4TH8v+xgBO1jThyiDAVO0pjSg7dYbG6Sf09kTU3HaYcXw4k9TEXjCNMWdH0UXdNW0FqfAS1dxXyEstPMPtKA0pKwflyBUIcaFY6LlYLS6vKPFjxDZCnaBM8DAdsV4g1/pKhWsIJuwSOxJFZ6ym07m9EPOZZJzRDT6743gjvFo/li5063StNl1gaOhaPdC1W3R/8FDiM75uOd2LZZ3RTZG3ERsquxbuYjGd6uCOzBV0nAe7rvM8uNDV7OkF35OML/eEh0uP7hAjKPFwCyn2RZxTTcp4SAOaoGheqRPBhB3/KOanxl/ZGLp20Z4dzu/Ojqr1LlWOWICVBJUm5/EhZ4YjxMmuHo911NSjb5SI11bO37APtEb1ITK6ZR5WTzXYl9PoZI4i0tfNQWKZYj7Cn9DkI95mL7yB+zm8LWjXeJtuw9HzSbV4twYqd0gxEIQPM9xIZ7iUZviqbVTY8UD4yYxDW7WkHYG6rmwoG8dzg/Zq+5xO3fY37T9lji/GEICimrvXR9y6j0B4rY8UHel6neF7zQ+0YRSxJUfBciUrgGV0Z95/LwG164HTr5/UZBmNHWRqcpOtyU12H7kJ6KDizfp2lirNP4Wl06xQwWey1MYnD8WYoSVJ1vSe9NJPOv+s9NLKCna3dK7JU/WBwMjuFINuiC8W1JjvssE6hXY4btvC6r206uyo49ZKxWEKR5i3V4yyND+wr8lB7vlJihkRb8Ry94phxQkfv1XSRMSMIBG3PIHjJJQHaumkPsrCMnJk4iBBkuAXTYUAhZDXYOX/GYkDsZSIrGSc9ySCpU+YmA6V6134iCMZ97Nhtgt6Oia447q7mbI8aEYQXQCxxvfbxWofw14pEgdbg56KGgPxk3EQ49u0i2+WuEFXR7WeWZM26g3gjIeN3d5fMWlpE1+ktsEyRqnIgMi/Of095ZQLx13c92EJ0HJ0TGUxcXWRfuGsC+rJ8VJu1Nf6ZXmvrfW7xfUnbpfm9yJ4a451mmdv223uu5uzrGhW307z7tUHaDj/Dw==</diagram></mxfile>" > - - + + - Landmark Based - Localizer + Landmark Based + Localizer - + - Other Autoware - packages + Other Autoware + packages - - + + - - /twist_with_covariance + + /twist_with_covariance - - + + - - /image + + /image - - + + - - /pose_with_covariance + + /pose_with_covariance - + - /ar_tag_based_localizer + /ar_tag_based_localizer - + - /ekf_localizer + /ekf_localizer - - + + - - /camera_info + + /camera_info - - + + - - Lanelet2 + + Lanelet2 - - + + - - /tf_static + + Load once at startup - + - /landmark_tf_caster + /lanelet2_map_loader - - - - - /lanelet2_map_loader - - - + + - - /ekf_pose_with_covariance + + /ekf_pose_with_covariance diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt similarity index 69% rename from localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt rename to localization/landmark_based_localizer/landmark_parser/CMakeLists.txt index 8d8cb546b6162..41f7c00d61383 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt +++ b/localization/landmark_based_localizer/landmark_parser/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(landmark_tf_caster) +project(landmark_parser) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,13 +12,10 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_executable(landmark_tf_caster - src/landmark_tf_caster_node.cpp - src/landmark_tf_caster_core.cpp +ament_auto_add_library(landmark_parser + src/landmark_parser_core.cpp ) ament_auto_package( INSTALL_TO_SHARE - launch - config ) diff --git a/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp b/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp new file mode 100644 index 0000000000000..edf45c2a2997a --- /dev/null +++ b/localization/landmark_based_localizer/landmark_parser/include/landmark_parser/landmark_parser_core.hpp @@ -0,0 +1,34 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ +#define LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ + +#include + +#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" +#include +#include + +#include +#include + +std::map parse_landmark( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const std::string & target_subtype, const rclcpp::Logger & logger); + +visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( + const std::map & landmarks); + +#endif // LANDMARK_PARSER__LANDMARK_PARSER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_parser/package.xml similarity index 69% rename from localization/landmark_based_localizer/landmark_tf_caster/package.xml rename to localization/landmark_based_localizer/landmark_parser/package.xml index 75f42b91f502a..e3daa93f81220 100644 --- a/localization/landmark_based_localizer/landmark_tf_caster/package.xml +++ b/localization/landmark_based_localizer/landmark_parser/package.xml @@ -1,22 +1,24 @@ - landmark_tf_caster + landmark_parser 0.0.0 - The landmark_tf_caster package + The landmark_parser package Shintaro Sakoda Masahiro Sakamoto + Yamato Ando + Kento Yabuuchi Apache License 2.0 ament_cmake autoware_cmake + eigen + autoware_auto_mapping_msgs + geometry_msgs lanelet2_extension rclcpp - tf2_eigen - tf2_geometry_msgs - tf2_ros ament_cmake diff --git a/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp b/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp new file mode 100644 index 0000000000000..c86b35b6115b4 --- /dev/null +++ b/localization/landmark_based_localizer/landmark_parser/src/landmark_parser_core.cpp @@ -0,0 +1,154 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "landmark_parser/landmark_parser_core.hpp" + +#include "lanelet2_extension/utility/message_conversion.hpp" + +#include + +#include +#include + +std::map parse_landmark( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const std::string & target_subtype, const rclcpp::Logger & logger) +{ + RCLCPP_INFO_STREAM(logger, "msg->format_version: " << msg->format_version); + RCLCPP_INFO_STREAM(logger, "msg->map_format: " << msg->map_format); + RCLCPP_INFO_STREAM(logger, "msg->map_version: " << msg->map_version); + RCLCPP_INFO_STREAM(logger, "msg->data.size(): " << msg->data.size()); + lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; + lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); + + std::map landmark_map; + + for (const auto & poly : lanelet_map_ptr->polygonLayer) { + const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; + if (type != "pose_marker") { + continue; + } + const std::string subtype{poly.attributeOr(lanelet::AttributeName::Subtype, "none")}; + if (subtype != target_subtype) { + continue; + } + + // Get marker_id + const std::string marker_id = poly.attributeOr("marker_id", "none"); + + // Get 4 vertices + const auto & vertices = poly.basicPolygon(); + if (vertices.size() != 4) { + RCLCPP_WARN_STREAM(logger, "vertices.size() (" << vertices.size() << ") != 4"); + continue; + } + + // 4 vertices represent the marker vertices in counterclockwise order + // Calculate the volume by considering it as a tetrahedron + const auto & v0 = vertices[0]; + const auto & v1 = vertices[1]; + const auto & v2 = vertices[2]; + const auto & v3 = vertices[3]; + const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; + RCLCPP_INFO_STREAM(logger, "volume = " << volume); + const double volume_threshold = 1e-5; + if (volume > volume_threshold) { + RCLCPP_WARN_STREAM( + logger, + "volume (" << volume << ") > threshold (" << volume_threshold << "), This is not plane."); + continue; + } + + // Calculate the center of the quadrilateral + const auto center = (v0 + v1 + v2 + v3) / 4.0; + + // Define axes + const auto x_axis = (v1 - v0).normalized(); + const auto y_axis = (v2 - v1).normalized(); + const auto z_axis = x_axis.cross(y_axis).normalized(); + + // Construct rotation matrix and convert it to quaternion + Eigen::Matrix3d rotation_matrix; + rotation_matrix << x_axis, y_axis, z_axis; + const Eigen::Quaterniond q{rotation_matrix}; + + // Create Pose + geometry_msgs::msg::Pose pose; + pose.position.x = center.x(); + pose.position.y = center.y(); + pose.position.z = center.z(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + + // Add to map + landmark_map[marker_id] = pose; + RCLCPP_INFO_STREAM(logger, "id: " << marker_id); + RCLCPP_INFO_STREAM( + logger, + "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); + RCLCPP_INFO_STREAM( + logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " + << pose.orientation.z << ", " << pose.orientation.w); + } + + return landmark_map; +} + +visualization_msgs::msg::MarkerArray convert_to_marker_array_msg( + const std::map & landmarks) +{ + int32_t id = 0; + visualization_msgs::msg::MarkerArray marker_array; + for (const auto & [id_str, pose] : landmarks) { + // publish cube as a thin board + visualization_msgs::msg::Marker cube_marker; + cube_marker.header.frame_id = "map"; + cube_marker.header.stamp = rclcpp::Clock().now(); + cube_marker.ns = "landmark_cube"; + cube_marker.id = id; + cube_marker.type = visualization_msgs::msg::Marker::CUBE; + cube_marker.action = visualization_msgs::msg::Marker::ADD; + cube_marker.pose = pose; + cube_marker.scale.x = 1.0; + cube_marker.scale.y = 2.0; + cube_marker.scale.z = 0.1; + cube_marker.color.a = 0.5; + cube_marker.color.r = 0.0; + cube_marker.color.g = 1.0; + cube_marker.color.b = 0.0; + marker_array.markers.push_back(cube_marker); + + // publish text + visualization_msgs::msg::Marker text_marker; + text_marker.header.frame_id = "map"; + text_marker.header.stamp = rclcpp::Clock().now(); + text_marker.ns = "landmark_text"; + text_marker.id = id; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + text_marker.pose = pose; + text_marker.text = "(" + id_str + ")"; + text_marker.scale.z = 0.5; + text_marker.color.a = 1.0; + text_marker.color.r = 1.0; + text_marker.color.g = 0.0; + text_marker.color.b = 0.0; + marker_array.markers.push_back(text_marker); + + id++; + } + return marker_array; +} diff --git a/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml b/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml deleted file mode 100644 index 2fe2869038e82..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - # Tags with a volume greater than this value will not cast their pose - volume_threshold: 1e-5 # [m^3] diff --git a/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp deleted file mode 100644 index 0eb6706a0b5b0..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp +++ /dev/null @@ -1,49 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ -#define LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ - -#include - -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" -#include - -#include -#include -#include - -#include - -class LandmarkTfCaster : public rclcpp::Node -{ -public: - LandmarkTfCaster(); - -private: - void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); - void publish_tf(const lanelet::Polygon3d & poly); - - // Parameters - double volume_threshold_; - - // tf - std::unique_ptr tf_buffer_; - std::unique_ptr tf_broadcaster_; - - // Subscribers - rclcpp::Subscription::SharedPtr map_bin_sub_; -}; - -#endif // LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml deleted file mode 100644 index eeaba555cb6f5..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp deleted file mode 100644 index 801e036415326..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "landmark_tf_caster/landmark_tf_caster_core.hpp" - -#include "lanelet2_extension/utility/message_conversion.hpp" - -#include -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -LandmarkTfCaster::LandmarkTfCaster() : Node("landmark_tf_caster") -{ - // Parameters - volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5); - - // tf - tf_buffer_ = std::make_unique(this->get_clock()); - tf_broadcaster_ = std::make_unique(this); - - // Subscribers - map_bin_sub_ = this->create_subscription( - "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&LandmarkTfCaster::map_bin_callback, this, std::placeholders::_1)); - RCLCPP_INFO(this->get_logger(), "finish setup"); -} - -void LandmarkTfCaster::map_bin_callback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) -{ - RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version); - RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_format: " << msg->map_format); - RCLCPP_INFO_STREAM(this->get_logger(), "msg->map_version: " << msg->map_version); - RCLCPP_INFO_STREAM(this->get_logger(), "msg->data.size(): " << msg->data.size()); - lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - for (const auto & poly : lanelet_map_ptr->polygonLayer) { - const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; - if (type != "pose_marker") { - continue; - } - publish_tf(poly); - } -} - -void LandmarkTfCaster::publish_tf(const lanelet::Polygon3d & poly) -{ - // Get marker_id - const std::string marker_id = poly.attributeOr("marker_id", "none"); - - // Get 4 vertices - const auto & vertices = poly.basicPolygon(); - if (vertices.size() != 4) { - RCLCPP_WARN_STREAM(this->get_logger(), "vertices.size() (" << vertices.size() << ") != 4"); - return; - } - - // 4 vertices represent the marker vertices in counterclockwise order - // Calculate the volume by considering it as a tetrahedron - const auto & v0 = vertices[0]; - const auto & v1 = vertices[1]; - const auto & v2 = vertices[2]; - const auto & v3 = vertices[3]; - const double volume = (v1 - v0).cross(v2 - v0).dot(v3 - v0) / 6.0; - RCLCPP_INFO_STREAM(this->get_logger(), "volume = " << volume); - if (volume > volume_threshold_) { - RCLCPP_WARN_STREAM( - this->get_logger(), - "volume (" << volume << ") > threshold (" << volume_threshold_ << "), This is not plane."); - return; - } - - // Calculate the center of the quadrilateral - const auto center = (v0 + v1 + v2 + v3) / 4.0; - - // Define axes - const auto x_axis = (v1 - v0).normalized(); - const auto y_axis = (v2 - v1).normalized(); - const auto z_axis = x_axis.cross(y_axis).normalized(); - - // Construct rotation matrix and convert it to quaternion - Eigen::Matrix3d rotation_matrix; - rotation_matrix << x_axis, y_axis, z_axis; - const Eigen::Quaterniond q{rotation_matrix}; - - // Create transform - geometry_msgs::msg::TransformStamped tf; - tf.header.stamp = this->now(); - tf.header.frame_id = "map"; - tf.child_frame_id = "tag_" + marker_id; - tf.transform.translation.x = center.x(); - tf.transform.translation.y = center.y(); - tf.transform.translation.z = center.z(); - tf.transform.rotation.x = q.x(); - tf.transform.rotation.y = q.y(); - tf.transform.rotation.z = q.z(); - tf.transform.rotation.w = q.w(); - - // Publish transform - tf_broadcaster_->sendTransform(tf); - RCLCPP_INFO_STREAM(this->get_logger(), "id: " << marker_id); - RCLCPP_INFO_STREAM( - this->get_logger(), "(x, y, z) = " << tf.transform.translation.x << ", " - << tf.transform.translation.y << ", " - << tf.transform.translation.z); - RCLCPP_INFO_STREAM( - this->get_logger(), "q = " << tf.transform.rotation.x << ", " << tf.transform.rotation.y << ", " - << tf.transform.rotation.z << ", " << tf.transform.rotation.w); -} diff --git a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp deleted file mode 100644 index 4c34e593e7552..0000000000000 --- a/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp +++ /dev/null @@ -1,22 +0,0 @@ -// Copyright 2023 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "landmark_tf_caster/landmark_tf_caster_core.hpp" - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); -} From b4d5d90af6805e5cf5dc0e5d7d901020c0f0d7b7 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Oct 2023 20:05:32 +0900 Subject: [PATCH 181/206] refactor(goal_planner): use updateData (#5416) Signed-off-by: kosuke55 upda --- .../goal_planner/goal_planner_module.hpp | 2 +- .../goal_planner/goal_planner_module.cpp | 45 +++++++++++-------- 2 files changed, 27 insertions(+), 20 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 343302a022a69..8b722d3316093 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -225,8 +225,8 @@ class GoalPlannerModule : public SceneModuleInterface [[deprecated]] ModuleStatus updateState() override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; - void processOnEntry() override; void processOnExit() override; + void updateData() override; void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 241a820175ffe..2db3560d6e4f3 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -114,6 +114,13 @@ GoalPlannerModule::GoalPlannerModule( freespace_parking_timer_cb_group_); } + // Initialize safety checker + if (parameters_->safety_check_params.enable_safety_check) { + initializeSafetyCheckParameters(); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); + } + status_.reset(); } @@ -232,6 +239,25 @@ BehaviorModuleOutput GoalPlannerModule::run() return plan(); } +void GoalPlannerModule::updateData() +{ + // Initialize Occupancy Grid Map + // This operation requires waiting for `planner_data_`, hence it is executed here instead of in + // the constructor. Ideally, this operation should only need to be performed once. + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { + initializeOccupancyGridMap(); + } + + updateOccupancyGrid(); + + // set current road lanes, pull over lanes, and drivable lane + setLanes(); + + generateGoalCandidates(); +} + void GoalPlannerModule::initializeOccupancyGridMap() { OccupancyGridMapParam occupancy_grid_map_param{}; @@ -256,22 +282,6 @@ void GoalPlannerModule::initializeSafetyCheckParameters() objects_filtering_params_, parameters_); } -void GoalPlannerModule::processOnEntry() -{ - // Initialize occupancy grid map - if ( - parameters_->use_occupancy_grid_for_goal_search || - parameters_->use_occupancy_grid_for_path_collision_check) { - initializeOccupancyGridMap(); - } - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - goal_planner_data_.collision_check); - } -} - void GoalPlannerModule::processOnExit() { resetPathCandidate(); @@ -562,8 +572,6 @@ BehaviorModuleOutput GoalPlannerModule::plan() resetPathCandidate(); resetPathReference(); - generateGoalCandidates(); - path_reference_ = getPreviousModuleOutput().reference_path; if (goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { @@ -984,7 +992,6 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( { waitApproval(); - updateOccupancyGrid(); BehaviorModuleOutput out; out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); From f41abb37a6b7e2b00f2cc0f0efc762c34c73cc8a Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Oct 2023 22:11:31 +0900 Subject: [PATCH 182/206] feat(goal_planner): calculate stop pose from closest goal candidate (#5410) * feat(goal_planner): calculate stop pose from closest goal candidate Signed-off-by: kosuke55 * style(pre-commit): autofix * Update planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp Co-authored-by: Kyoichi Sugahara * style(pre-commit): autofix * Update planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp * style(pre-commit): autofix --------- Signed-off-by: kosuke55 Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Kyoichi Sugahara --- .../goal_planner/goal_planner_module.hpp | 2 ++ .../utils/goal_planner/goal_searcher.hpp | 2 ++ .../utils/goal_planner/goal_searcher_base.hpp | 2 ++ .../goal_planner/goal_planner_module.cpp | 25 ++++++++++++------ .../src/utils/goal_planner/goal_searcher.cpp | 26 +++++++++++++++++++ 5 files changed, 49 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index 8b722d3316093..c5b2209c16f19 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -142,6 +142,7 @@ class PullOverStatus DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) DEFINE_SETTER_GETTER(std::vector, pull_over_path_candidates) DEFINE_SETTER_GETTER(std::optional, closest_start_pose) @@ -174,6 +175,7 @@ class PullOverStatus std::optional modified_goal_pose_; Pose refined_goal_pose_{}; GoalCandidates goal_candidates_{}; + Pose closest_goal_candidate_pose_{}; // pull over path std::vector pull_over_path_candidates_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp index 2ed58b9678e70..2fc0acf1c5086 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher.hpp @@ -35,6 +35,8 @@ class GoalSearcher : public GoalSearcherBase GoalCandidates search() override; void update(GoalCandidates & goal_candidates) const override; + GoalCandidate getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const override; private: void countObjectsToAvoid( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index ab319111f6da6..24c614e072b8f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -58,6 +58,8 @@ class GoalSearcherBase MultiPolygon2d getAreaPolygons() { return area_polygons_; } virtual GoalCandidates search() = 0; virtual void update([[maybe_unused]] GoalCandidates & goal_candidates) const { return; } + virtual GoalCandidate getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const = 0; protected: GoalPlannerParameters parameters_{}; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 2db3560d6e4f3..f1118f2b7882a 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -557,6 +557,11 @@ void GoalPlannerModule::generateGoalCandidates() goal_searcher_->setPlannerData(planner_data_); goal_searcher_->setReferenceGoal(status_.get_refined_goal_pose()); status_.set_goal_candidates(goal_searcher_->search()); + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length, false); + status_.set_closest_goal_candidate_pose( + goal_searcher_->getClosetGoalCandidateAlongLanes(status_.get_goal_candidates()).goal_pose); } else { GoalCandidate goal_candidate{}; goal_candidate.goal_pose = goal_pose; @@ -564,6 +569,7 @@ void GoalPlannerModule::generateGoalCandidates() GoalCandidates goal_candidates{}; goal_candidates.push_back(goal_candidate); status_.set_goal_candidates(goal_candidates); + status_.set_closest_goal_candidate_pose(goal_pose); } } @@ -1089,20 +1095,23 @@ PathWithLaneId GoalPlannerModule::generateStopPath() // difference between the outer and inner sides) // 4. feasible stop const auto search_start_offset_pose = calcLongitudinalOffsetPose( - reference_path.points, status_.get_refined_goal_pose().position, - -parameters_->backward_goal_search_length - common_parameters.base_link2front - - approximate_pull_over_distance_); + reference_path.points, status_.get_closest_goal_candidate_pose().position, + -approximate_pull_over_distance_); if ( !status_.get_is_safe_static_objects() && !status_.get_closest_start_pose() && !search_start_offset_pose) { return generateFeasibleStopPath(); } - const Pose stop_pose = - status_.get_is_safe_static_objects() - ? status_.get_pull_over_path()->start_pose - : (status_.get_closest_start_pose() ? status_.get_closest_start_pose().value() - : *search_start_offset_pose); + const Pose stop_pose = [&]() -> Pose { + if (status_.get_is_safe_static_objects()) { + return status_.get_pull_over_path()->start_pose; + } + if (status_.get_closest_start_pose()) { + return status_.get_closest_start_pose().value(); + } + return *search_start_offset_pose; + }(); // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(reference_path, stop_pose); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 1e6dc1776359b..f498ecd9ed8ec 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -468,4 +468,30 @@ bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons return false; } +GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( + const GoalCandidates & goal_candidates) const +{ + const auto current_lanes = utils::getExtendedCurrentLanes( + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, + /*forward_only_in_route*/ false); + + // Define a lambda function to compute the arc length for a given goal candidate. + auto getGoalArcLength = [¤t_lanes](const auto & candidate) { + return lanelet::utils::getArcCoordinates(current_lanes, candidate.goal_pose).length; + }; + + // Find the closest goal candidate by comparing the arc lengths of each candidate. + const auto closest_goal_candidate = std::min_element( + goal_candidates.begin(), goal_candidates.end(), + [&getGoalArcLength](const auto & a, const auto & b) { + return getGoalArcLength(a) < getGoalArcLength(b); + }); + + if (closest_goal_candidate == goal_candidates.end()) { + return {}; // return empty GoalCandidate in case no valid candidates are found. + } + + return *closest_goal_candidate; +} + } // namespace behavior_path_planner From dfc83a0beb913be92f57157c7cbc433337effba3 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 26 Oct 2023 22:12:04 +0900 Subject: [PATCH 183/206] feat(goal_planner): do not decelerate before path candidates generation (#5412) refactor(goal_planner): use updateData upda use getPreviousModuleOutput also in waiting approval Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index f1118f2b7882a..32b54a4e39170 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -914,14 +914,16 @@ void GoalPlannerModule::decideVelocity() BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { + // if pull over path candidates generation is not finished, use previous module output + if (status_.get_pull_over_path_candidates().empty()) { + return getPreviousModuleOutput(); + } + constexpr double path_update_duration = 1.0; resetPathCandidate(); resetPathReference(); - // set current road lanes, pull over lanes, and drivable lane - setLanes(); - // Check if it needs to decide path status_.set_has_decided_path(hasDecidedPath()); @@ -996,6 +998,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification() { + // if pull over path candidates generation is not finished, use previous module output + if (status_.get_pull_over_path_candidates().empty()) { + return getPreviousModuleOutput(); + } + waitApproval(); BehaviorModuleOutput out; @@ -1038,7 +1045,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( std::pair GoalPlannerModule::calcDistanceToPathChange() const { - const auto & full_path = status_.get_pull_over_path()->getFullPath(); + const auto full_path = status_.get_pull_over_path()->getFullPath(); const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), From a1cbdcb2d4c3574f40520c478bcaba3d9e85b1f9 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 26 Oct 2023 22:13:58 +0900 Subject: [PATCH 184/206] fix(lane_change): filter objects for skip parking objects (#5419) * fix(lane_change): filter objects for skip parking objects Signed-off-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Kosuke Takeuchi --------- Signed-off-by: Fumiya Watanabe Co-authored-by: Kosuke Takeuchi --- .../scene_module/lane_change/normal.hpp | 3 ++ .../src/scene_module/lane_change/normal.cpp | 28 ++++++++++++++++--- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 54883489f2fe6..dc9f44fbd53da 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -155,6 +155,9 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + std::vector filterObjectsInTargetLane( + const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const; + //! @brief Check if the ego vehicle is in stuck by a stationary obstacle. //! @param obstacle_check_distance Distance to check ahead for any objects that might be //! obstructing ego path. It makes sense to use values like the maximum lane change distance. diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index de120592e7505..d887b570b753a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -918,6 +918,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return filtered_obj_indices; } +std::vector NormalLaneChange::filterObjectsInTargetLane( + const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const +{ + const auto target_polygon = + utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + std::vector filtered_objects{}; + if (target_polygon) { + for (auto & obj : objects.target_lane) { + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + if (boost::geometry::intersects(target_polygon.value(), obj_polygon)) { + filtered_objects.push_back(obj); + } + } + } + + return filtered_objects; +} + PathWithLaneId NormalLaneChange::getTargetSegment( const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, const double lane_changing_length, @@ -1296,11 +1314,13 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); + + std::vector filtered_objects = + filterObjectsInTargetLane(target_objects, target_lanes); if ( - !is_stuck && - utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_, object_debug_)) { + !is_stuck && utils::lane_change::passParkedObject( + route_handler, *candidate_path, filtered_objects, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_, object_debug_)) { debug_print( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); From a995f98cd42b50862d4b33c7d6648e4219d5f58d Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 26 Oct 2023 22:34:55 +0900 Subject: [PATCH 185/206] fix(avoidance): calculate road shoulder distance for prev and next lanelet (#5226) Signed-off-by: satoshi-ota --- .../src/utils/avoidance/utils.cpp | 23 +++++++++++++++---- 1 file changed, 18 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index a9f40dad86ae4..725b528abad51 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -1037,25 +1037,38 @@ void filterTargetObjects( }; // current lanelet - update_road_to_shoulder_distance(overhang_lanelet); + { + update_road_to_shoulder_distance(overhang_lanelet); + + o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, + overhang_basic_pose, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); + } + // previous lanelet lanelet::ConstLanelets previous_lanelet{}; if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) { update_road_to_shoulder_distance(previous_lanelet.front()); + + o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, o.to_road_shoulder_distance, previous_lanelet.front(), + o.overhang_pose.position, overhang_basic_pose, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); } + // next lanelet lanelet::ConstLanelet next_lanelet{}; if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) { update_road_to_shoulder_distance(next_lanelet); - } - debug.bounds.push_back(target_line); - { o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, + rh, target_line, o.to_road_shoulder_distance, next_lanelet, o.overhang_pose.position, overhang_basic_pose, parameters->use_hatched_road_markings, parameters->use_intersection_areas); } + + debug.bounds.push_back(target_line); } // calculate avoid_margin dynamically From 27f28d679e226f53a9ce3c7501a61dcab783bfa5 Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Fri, 27 Oct 2023 00:38:12 +0900 Subject: [PATCH 186/206] chore(radar_static_pointcloud_filter): fix copyright (#5316) Signed-off-by: scepter914 --- .../radar_static_pointcloud_filter.param.yaml | 14 -------------- .../radar_static_pointcloud_filter_node.cpp | 2 +- 2 files changed, 1 insertion(+), 15 deletions(-) diff --git a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml index a429f7a065ffe..01f3a4cdd641a 100644 --- a/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml +++ b/sensing/radar_static_pointcloud_filter/config/radar_static_pointcloud_filter.param.yaml @@ -1,17 +1,3 @@ -# Copyright 2023 Foxconn, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - /**: ros__parameters: doppler_velocity_sd: 4.0 diff --git a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp index 772dcef753c11..0eda1eae627e7 100644 --- a/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp +++ b/sensing/radar_static_pointcloud_filter/src/radar_static_pointcloud_filter_node/radar_static_pointcloud_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2022-2023 Foxconn, TIER IV, Inc. +// Copyright 2022 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From e16ebd850a5fc44c902dd50d6b883b323bae6519 Mon Sep 17 00:00:00 2001 From: Alexey Panferov <37497658+lexavtanke@users.noreply.github.com> Date: Thu, 26 Oct 2023 18:50:13 +0300 Subject: [PATCH 187/206] build(tvm_utility): remove download logic from CMake and update documentation (#4923) * add include tier4_autoware_utils and dependency Signed-off-by: Alexey Panferov * remove downloading logic from Cmake, update documentation Signed-off-by: Alexey Panferov * build(tvm_utility): remove downloading logic from Cmake, update documentation Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): fix lint_cmake error Signed-off-by: Alexey Panferov * build(tvm_utility): format warning message Signed-off-by: Alexey Panferov * build(tvm_utility): add logic to work with autoware_data folder, add nn config header and test image Signed-off-by: Alexey Panferov * style(pre-commit): autofix * style(pre-commit): autofix * build(tvm_utility): refactor, update InferenceEngineTVM constructor Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): add lightweight model and test with it Signed-off-by: Alexey Panferov * build(tvm_utility): make building yolo_v2_tiny disable by default Signed-off-by: Alexey Panferov * build(tvm_utility): remove test artifact for yolo_v2_tiny Signed-off-by: Alexey Panferov * build(tvm_utility): update docs Signed-off-by: Alexey Panferov * build(tvm_utility): update docs Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): update namespace in abs_model test Signed-off-by: Alexey Panferov * build(tvm_utility): rewrite yolo_v2_tiny as example Signed-off-by: Alexey Panferov * build(tvm_utility): clean comments in yolo_v2_tiny example main.cpp Signed-off-by: Alexey Panferov * build(tvm_utility): add launch file for yolo_v2_tiny example Signed-off-by: Alexey Panferov * build(tvm_utility): update yolo_v2_tiny example readme Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): add model for arm based systems, need to be tested on device Signed-off-by: Alexey Panferov * style(pre-commit): autofix * style(pre-commit): autofix * build(tvm_utility): update config header for arm Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): remove debug output Signed-off-by: Alexey Panferov * build(tvm_utility): add find_package conditional section Signed-off-by: Alexey Panferov * build(tvm_utility): fix lint_cmake errors Signed-off-by: Alexey Panferov * build(tvm_utility): remove coping model files during build Signed-off-by: Alexey Panferov * build(tvm_utility): update readme with new data folder structure Signed-off-by: Alexey Panferov * build(tvm_utility): fix spell check warnings Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): add no model files guard to get_neural_network Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): set back default paths in config headers Signed-off-by: Alexey Panferov * build(tvm_utility): add param file, update launch file Signed-off-by: Alexey Panferov * build(tvm_utility): add schema file, update node name Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): fix json-schema-check Signed-off-by: Alexey Panferov * build(tvm_utility): fix json-schema-check Signed-off-by: Alexey Panferov * style(pre-commit): autofix * build(tvm_utility): add parameter table to example readme Signed-off-by: Alexey Panferov * build(tvm_utility): fix typo-error in description of schema.json Signed-off-by: Alexey Panferov * style(pre-commit): autofix * buiild(tvm_utility): fix spell-check warning and typo Signed-off-by: Alexey Panferov --------- Signed-off-by: Alexey Panferov Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .cspell-partial.json | 2 +- common/tvm_utility/.gitignore | 2 - common/tvm_utility/CMakeLists.txt | 80 ++++++++-- common/tvm_utility/README.md | 59 +++---- .../config/yolo_v2_tiny_example.param.yaml | 6 + .../abs_model_aarch64/deploy_graph.json | 36 +++++ .../models/abs_model_aarch64/deploy_lib.so | Bin 0 -> 14824 bytes .../abs_model_aarch64/deploy_param.params | Bin 0 -> 32 bytes .../inference_engine_tvm_config.hpp | 54 +++++++ .../models/abs_model_x86_64/deploy_graph.json | 36 +++++ .../models/abs_model_x86_64/deploy_lib.so | Bin 0 -> 18168 bytes .../abs_model_x86_64/deploy_param.params | Bin 0 -> 32 bytes .../inference_engine_tvm_config.hpp | 53 +++++++ .../inference_engine_tvm_config.hpp | 56 +++++++ .../{test => example}/yolo_v2_tiny/main.cpp | 88 +++++++---- .../include/tvm_utility/pipeline.hpp | 13 +- .../launch/yolo_v2_tiny_example.launch.xml | 23 +++ .../schema/yolo_v2_tiny_example.schema.json | 47 ++++++ common/tvm_utility/test/abs_model/main.cpp | 146 ++++++++++++++++++ .../tvm-utility-yolo-v2-tiny-tests.md | 45 ++++-- common/tvm_utility/tvm_utility-extras.cmake | 47 +----- 21 files changed, 649 insertions(+), 144 deletions(-) create mode 100644 common/tvm_utility/config/yolo_v2_tiny_example.param.yaml create mode 100644 common/tvm_utility/data/models/abs_model_aarch64/deploy_graph.json create mode 100755 common/tvm_utility/data/models/abs_model_aarch64/deploy_lib.so create mode 100644 common/tvm_utility/data/models/abs_model_aarch64/deploy_param.params create mode 100644 common/tvm_utility/data/models/abs_model_aarch64/inference_engine_tvm_config.hpp create mode 100644 common/tvm_utility/data/models/abs_model_x86_64/deploy_graph.json create mode 100644 common/tvm_utility/data/models/abs_model_x86_64/deploy_lib.so create mode 100644 common/tvm_utility/data/models/abs_model_x86_64/deploy_param.params create mode 100644 common/tvm_utility/data/models/abs_model_x86_64/inference_engine_tvm_config.hpp create mode 100644 common/tvm_utility/data/models/yolo_v2_tiny/inference_engine_tvm_config.hpp rename common/tvm_utility/{test => example}/yolo_v2_tiny/main.cpp (80%) create mode 100644 common/tvm_utility/launch/yolo_v2_tiny_example.launch.xml create mode 100644 common/tvm_utility/schema/yolo_v2_tiny_example.schema.json create mode 100644 common/tvm_utility/test/abs_model/main.cpp diff --git a/.cspell-partial.json b/.cspell-partial.json index f45ae8961c56b..13849ef298019 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -5,5 +5,5 @@ "perception/bytetrack/lib/**" ], "ignoreRegExpList": [], - "words": [] + "words": ["dltype", "tvmgen"] } diff --git a/common/tvm_utility/.gitignore b/common/tvm_utility/.gitignore index a09bb7234b379..e69de29bb2d1d 100644 --- a/common/tvm_utility/.gitignore +++ b/common/tvm_utility/.gitignore @@ -1,2 +0,0 @@ -artifacts/**/*.jpg -data/ diff --git a/common/tvm_utility/CMakeLists.txt b/common/tvm_utility/CMakeLists.txt index b49f141d9e2e4..c0a0d7385f615 100644 --- a/common/tvm_utility/CMakeLists.txt +++ b/common/tvm_utility/CMakeLists.txt @@ -29,15 +29,19 @@ set(TVM_UTILITY_NODE_LIB_HEADERS ament_auto_add_library(${PROJECT_NAME} SHARED ${TVM_UTILITY_NODE_LIB_HEADERS}) set_target_properties(${PROJECT_NAME} PROPERTIES LINKER_LANGUAGE CXX) -if(BUILD_TESTING) +set(BUILD_EXAMPLE OFF CACHE BOOL "enable build yolo_v2_tiny") + +if(BUILD_TESTING OR BUILD_EXAMPLE) + find_package(OpenCV REQUIRED) + set(tvm_runtime_DIR ${tvm_vendor_DIR}) + find_package(tvm_runtime CONFIG REQUIRED) # Get target backend set(${PROJECT_NAME}_BACKEND llvm CACHE STRING "${PROJECT_NAME} neural network backend") +endif() +if(BUILD_TESTING) # compile each folder inside test/ as a test case find_package(ament_cmake_gtest REQUIRED) - find_package(OpenCV REQUIRED) - set(tvm_runtime_DIR ${tvm_vendor_DIR}) - find_package(tvm_runtime CONFIG REQUIRED) set(TEST_ARTIFACTS "${CMAKE_CURRENT_LIST_DIR}/artifacts") file(GLOB TEST_CASES test/*) @@ -47,17 +51,11 @@ if(BUILD_TESTING) endif() # the folder name becomes the test case name file(RELATIVE_PATH TEST_CASE_NAME ${CMAKE_CURRENT_LIST_DIR}/test ${TEST_FOLDER}) - # Get neural network. set(NN_DEPENDENCY "") - get_neural_network(${TEST_CASE_NAME} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) + get_neural_network(${TEST_CASE_NAME}_${CMAKE_SYSTEM_PROCESSOR} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) if(NOT NN_DEPENDENCY STREQUAL "") - if(TEST_CASE_NAME STREQUAL "yolo_v2_tiny" AND - NOT EXISTS ${TEST_ARTIFACTS}/yolo_v2_tiny/test_image_0.jpg) - message(WARNING "Missing image artifact for yolo_v2_tiny, skipping test") - continue() - endif() # add all cpp files in the folder to the target file(GLOB TEST_CASE_SOURCES ${TEST_FOLDER}/*.cpp) ament_add_gtest(${TEST_CASE_NAME} ${TEST_CASE_SOURCES}) @@ -75,7 +73,7 @@ if(BUILD_TESTING) target_include_directories("${TEST_CASE_NAME}" SYSTEM PUBLIC "${OpenCV_INCLUDE_DIRS}" "${tvm_utility_FOUND_INCLUDE_DIRS}" - "data/models" + "data/models/${TEST_CASE_NAME}_${CMAKE_SYSTEM_PROCESSOR}" "include" ) @@ -93,5 +91,61 @@ if(BUILD_TESTING) endforeach() endif() +if(BUILD_EXAMPLE) + # compile each folder inside example/ as an example + find_package(rclcpp REQUIRED) + + set(EXAMPLE_ARTIFACTS "${CMAKE_CURRENT_LIST_DIR}/artifacts") + file(GLOB EXAMPLE_CASES example/*) + foreach(EXAMPLE_FOLDER ${EXAMPLE_CASES}) + if(NOT IS_DIRECTORY ${EXAMPLE_FOLDER}) + continue() + endif() + # the folder name becomes the example name + file(RELATIVE_PATH EXAMPLE_NAME ${CMAKE_CURRENT_LIST_DIR}/example ${EXAMPLE_FOLDER}) + # Get neural network. + set(NN_DEPENDENCY "") + get_neural_network(${EXAMPLE_NAME} ${${PROJECT_NAME}_BACKEND} NN_DEPENDENCY) + + if(NOT NN_DEPENDENCY STREQUAL "") + if(EXAMPLE_NAME STREQUAL "yolo_v2_tiny" AND + NOT EXISTS ${EXAMPLE_ARTIFACTS}/yolo_v2_tiny/test_image_0.jpg) + message(WARNING "Missing image artifact for yolo_v2_tiny, skipping example") + continue() + endif() + # add all cpp files in the folder to the target + file(GLOB EXAMPLE_SOURCES ${EXAMPLE_FOLDER}/*.cpp) + ament_auto_add_executable(${EXAMPLE_NAME} ${EXAMPLE_SOURCES}) + ament_target_dependencies(${EXAMPLE_NAME} + "ament_index_cpp" + "tvm_vendor" + "rclcpp" + ) + add_dependencies(${EXAMPLE_NAME} ${NN_DEPENDENCY}) + + target_link_libraries("${EXAMPLE_NAME}" + "${OpenCV_LIBRARIES}" + "${tvm_runtime_LIBRARIES}" + ) + + target_include_directories("${EXAMPLE_NAME}" SYSTEM PUBLIC + "${OpenCV_INCLUDE_DIRS}" + "${tvm_utility_FOUND_INCLUDE_DIRS}" + "data/models" + "include" + ) + + else() + message(WARNING "No model is generated for ${EXAMPLE_FOLDER} example") + endif() + + endforeach() +endif() + list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS "${PROJECT_NAME}-extras.cmake") -ament_auto_package() +# ament_auto_package() +ament_auto_package( + INSTALL_TO_SHARE + launch + config + artifacts) diff --git a/common/tvm_utility/README.md b/common/tvm_utility/README.md index 7d4874d5ed89a..4751428353886 100644 --- a/common/tvm_utility/README.md +++ b/common/tvm_utility/README.md @@ -41,7 +41,7 @@ The earliest supported version depends on each package making use of the inferen #### Models -Dependent packages are expected to use the `get_neural_network` cmake function from this package in order to get the compiled TVM models. +Dependent packages are expected to use the `get_neural_network` cmake function from this package in order to build proper external dependency. ### Error detection and handling @@ -50,76 +50,55 @@ error description. ### Neural Networks Provider -This package also provides a utility to get pre-compiled neural networks to packages using them for their inference. - The neural networks are compiled as part of the [Model Zoo](https://github.com/autowarefoundation/modelzoo/) CI pipeline and saved to an S3 bucket. -This package exports cmake variables and functions for ease of access to those neural networks. The `get_neural_network` function creates an abstraction for the artifact management. -The artifacts are saved under the source directory of the package making use of the function; under "data/". -Priority is given to user-provided files, under "data/user/${MODEL_NAME}/". -If there are no user-provided files, the function tries to reuse previously-downloaded artifacts. -If there are no previously-downloaded artifacts, and if the `DOWNLOAD_ARTIFACTS` cmake variable is set, they will be downloaded from the bucket. -Otherwise, nothing happens. +Users should check if model configuration header file is under "data/user/${MODEL_NAME}/". Otherwise, nothing happens and compilation of the package will be skipped. The structure inside of the source directory of the package making use of the function is as follow: ```{text} . ├── data -│ ├── downloads -│ │ ├── ${MODEL 1}-${ARCH 1}-{BACKEND 1}-{VERSION 1}.tar.gz -│ │ ├── ... -│ │ └── ${MODEL ...}-${ARCH ...}-{BACKEND ...}-{VERSION ...}.tar.gz -│ ├── models -│ │ ├── ${MODEL 1} -│ │ │ ├── ... -│ │ │ └── inference_engine_tvm_config.hpp -│ │ ├── ... -│ │ └── ${MODEL ...} -│ │ └── ... -│ └── user +│ └── models │ ├── ${MODEL 1} -│ │ ├── deploy_graph.json -│ │ ├── deploy_lib.so -│ │ ├── deploy_param.params │ │ └── inference_engine_tvm_config.hpp │ ├── ... │ └── ${MODEL ...} │ └── ... ``` -The `inference_engine_tvm_config.hpp` file needed for compilation by dependent packages is made available under "data/models/${MODEL_NAME}/inference_engine_tvm_config.hpp". +The `inference_engine_tvm_config.hpp` file needed for compilation by dependent packages should be available under "data/models/${MODEL_NAME}/inference_engine_tvm_config.hpp". Dependent packages can use the cmake `add_dependencies` function with the name provided in the `DEPENDENCY` output parameter of `get_neural_network` to ensure this file is created before it gets used. The other `deploy_*` files are installed to "models/${MODEL_NAME}/" under the `share` directory of the package. -The target version to be downloaded can be overwritten by setting the `MODELZOO_VERSION` cmake variable. - -#### Assumptions / Known limits - -If several packages make use of the same neural network, it will be downloaded once per package. - -In case a requested artifact doesn't exist in the S3 bucket, the error message from ExternalProject is not explicit enough for the user to understand what went wrong. +The other model files should be stored in autoware_data folder under package folder with the structure: -In case the user manually sets `MODELZOO_VERSION` to "latest", the archive will not be re-downloaded when it gets updated in the S3 bucket (it is not a problem for tagged versions as they are not expected to be updated). +```{text} +$HOME/autoware_data +| └──${package} +| └──models +| ├── ${MODEL 1} +| | ├── deploy_graph.json +| | ├── deploy_lib.so +| | └── deploy_param.params +| ├── ... +| └── ${MODEL ...} +| └── ... +``` #### Inputs / Outputs -Inputs: - -- `DOWNLOAD_ARTIFACTS` cmake variable; needs to be set to enable downloading the artifacts -- `MODELZOO_VERSION` cmake variable; can be used to overwrite the default target version of downloads - Outputs: -- `get_neural_network` cmake function; can be used to get a neural network compiled for a specific backend +- `get_neural_network` cmake function; create proper external dependency for a package with use of the model provided by the user. In/Out: - The `DEPENDENCY` argument of `get_neural_network` can be checked for the outcome of the function. - It is an empty string when the neural network couldn't be made available. + It is an empty string when the neural network wasn't provided by the user. ## Security considerations diff --git a/common/tvm_utility/config/yolo_v2_tiny_example.param.yaml b/common/tvm_utility/config/yolo_v2_tiny_example.param.yaml new file mode 100644 index 0000000000000..b63e4a99f97f2 --- /dev/null +++ b/common/tvm_utility/config/yolo_v2_tiny_example.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + image_filename: $(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/test_image_0.jpg + label_filename: $(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/labels.txt + anchor_filename: $(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/anchors.csv + data_path: $(env HOME)/autoware_data diff --git a/common/tvm_utility/data/models/abs_model_aarch64/deploy_graph.json b/common/tvm_utility/data/models/abs_model_aarch64/deploy_graph.json new file mode 100644 index 0000000000000..b226c01747dca --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_aarch64/deploy_graph.json @@ -0,0 +1,36 @@ +{ + "nodes": [ + { + "op": "null", + "name": "a", + "inputs": [] + }, + { + "op": "tvm_op", + "name": "tvmgen_default_fused_abs", + "attrs": { + "num_outputs": "1", + "num_inputs": "1", + "flatten_data": "0", + "func_name": "tvmgen_default_fused_abs", + "hash": "1be44995aa501758" + }, + "inputs": [[0, 0, 0]] + } + ], + "arg_nodes": [0], + "heads": [[1, 0, 0]], + "attrs": { + "dltype": ["list_str", ["float32", "float32"]], + "device_index": ["list_int", [1, 1]], + "storage_id": ["list_int", [0, 1]], + "shape": [ + "list_shape", + [ + [2, 2], + [2, 2] + ] + ] + }, + "node_row_ptr": [0, 1, 2] +} diff --git a/common/tvm_utility/data/models/abs_model_aarch64/deploy_lib.so b/common/tvm_utility/data/models/abs_model_aarch64/deploy_lib.so new file mode 100755 index 0000000000000000000000000000000000000000..e1ad7cebad73440094dcfe44edb5c60cc3605f77 GIT binary patch literal 14824 zcmeHOeQX@Zb$@$DipNipl5HtgVlEXXAZ_V*q$rtG?Py7Tkg_F5lBq(8Q!MxHmgHIY zLEY^sVkBEqVgrIzpjKtnX^|Q(;y*0ME$Bc_ZJ?;AASm3pC}=nhRMbsqBQ6*_Ze-Xk zbf~23d$aTIcK7aha;pB(LL=S2ncuvxoqaPqJ2O1Keb)}3q5vm9{54Q%zSc%GS+N+B z86+AEKpkE?;fsP-vKAQ}xtdjWfhhXXPIVzIef|Og3 za!lvttuPJPYM=%w{oauEUfL=7m=Z6gJ?~pl%KzqeQ7oa`Y#5lb++LI;KW{X(O2&QC zUZr*k$@X9>Z%5qQZK%VKzmbRbL+jta{m5vW`SnxpSa-g(zv=R+jlceYOi+Cz{-|BZ zzk2*tO2pfMKYG4(_@jFxjPhic2?f!NwII7zEz@O2gNClT*}7MPdM`*TwEkCCo`CU`+U z)GO2wnD*nZ16F}5{p5IiRN`G89CUp$kxJ?r%Sc#{Op-O!ItvDtCFAlwO@9S=Vd!ylKq$Q0}|)w&N!D} zYq#S;K|S&j2n%x#ockbEyy3t(J_(<9;Lgyy=9cj~`( z^uoDoemHXnz%$p5UVL%Rum4;akeAC?|1`jC+x3xL>w8_*3D;zVR>&e$uGrt{wfcXq#W)JzQVj zenk(!p_9!p)&<(@?Hl3HVgro*+7EqQ9dF!Ozw(KF?MnTl8e&~Fc733+w0<1x8$ruW zwINSE9O`I-v8naiOnWUHdYAJ1wZZn2XovE9G0!~BTCQHvUf+V{i&z(^x8=#7K#i6& z6fKu%({iz9?RCI>)48E@AL0G|70Ub)Wenu|OO&~Qb`;DfZr5@jHN&e15j&39U$mfK z=+klHU5ovi~=8zpU!AA3zdFyUjn#SP2CDPCvAH_W{CzjT^Ob5mvtTLJc^3TDdA?Cw zo#!Wk>O7Yls`GrlxjK)wygE-{MRlI<-d2_;{~dViI*uPA*4`ho{g0{~3zSu7LJ>eGRmI=x^_9sD-xCW*j3Pt6$Ub7_8ZX{iN%a(|77f zcOs?Of5``9ANlXT`@xWQ?%$8z)pwy8dVhPgz3-=QJ@xf&l>eZ%;lb|5w6nV(d;Dww zDPnIusx+TIjlc0HwX0Y@gL*!ME6=xRPr}uM`TW=d`bEC*c+X?~P$pxhEp@_(#W$;6 z$!tP5(vunW{`=Lw?saOVckyPmz?qp!W#dsbY^r_WR@T+kg}Q>%p(tOXRoIdZo{Yv4YAmCgqA|=UwS##m zYsAYuq@}fP>?X@yS!}?8?!IpI%U@QzmX_G$l-SUR53#z;y+xVcP2Iipgyo$?t(j@l zdn@=zC*mo?+Taa2(M;&-hCU9m%#<;0VkD5tq4Z=nVJ1azP*`%EI%Q;3BdKPS8N-TY zCSurNkyH|!8E2#xc8jIddV;DFYwr>s;Isc-Js`Cn54w8kiMboWO2?vR<`Lo+Pos>N zl7(8T*!tK6B)HiXNZ9(3RASl?y)13acn`W>ainpiCy<^;`U29| zkj^8W{WPCnM7oF+mv{>6JIc5ALCul7lr>H50$Ytd-$R`tq?B6mM{8`aA*Lavq~cG* z->(oGMG8YL>Yvv<=N~@nd*~Pc`gRLJw1yeQUjXep*HcIzC9V4++1OZneOz7P!>{w_4y<3;aK60p3^PeH46U zi$r@fl$vZ(*l%l=<#E~+#KikuykE1%hQ)p%W$%<_-naR@r=H*0m!;wVxSmfD3-4F* zUdXiEyx~2TDW9!~_h{zqqS&jXEWPI`NikTI`!P~Xyzi-=-u;wljzdXqKZ7O>Gd?99 z4cKgONmAb1VSCr}w*RA&KQGJNt~gDx?@!j_o{K26pHl5jBfQUjvlKop+l49bvyE)s ziVHFO!`Y;jRR@Fp!O;5NtSI$9-WLjn`h&gQHV*foFZN@24s(6@tF(+MoTlwyyRibI z5q*VyGnxQ$yE^w-ea?cCqDTBR{zMaRz|6B$co8o&+50a3YLtEVBO#_5<>6r@#8RV- zjUcfZ_V1D?)UL-|ZOyVeVh5#7`BKZuT7OIHsx>X=hd=kqa9x|S0;ALy#Htm`wAKEW zoh!FsH}ThkZxt0#GiC*jiaNpH@|DI$>`u#;qrST3KpfD8H&%l?#b`lM{;Q>#2;D3G z6iDEDZJd@G5x`VmxZ$>j2bMj6DeyA=FQ#vctP3hu{vXcy@K}bT;AXa?NqVfrbWi1w z5({sDtCziW>7&aAy7bd!BV9Jxt%}!KnNwiTG&Me^W0E5DjLbe?gkF=`pA?~YWOlI# z$>@hBIT)d?PMPIl4X_H%W? zgT3tMeMN}<94|s89@6M&S5ud#j$=QD62`KCA447=bV>4~PJVAVEB)Bjxgp(n4e)!z zjn@LdU)=aI2+48Ujn@H>?`}K*{GM>*^@aC|8?SVZr4e|1b@MkB&RMzfX5ja+8*hPW z`JQ&;%M0g{+;}U5?r6{@P2mV zrRRC5-j8qT)oMBY6lbN$Uznx1>->e;i5KTtj2GwGix-!N%iJ*fd6x&Vi+*UYfU6}q z-DM4YocC575I|`>Jc76nXKnNHJAk0FP^(& zhUXBc=k-1L!7v~zE+8$}p8Q-29|V$r4)rLsGRPkaY@mXke-w60``6Eep3=B|SKuG# zz2oNxhy#?4+t;KX?|4{`uUiGj4exl{j<~PP`ANI~mcDnsg#0R*_M{=i*LpB2egknj z-{?KZ_)USAo|Ak6akY$|vxt{FKlyS6{=XCYi}BCjY0#6+qne7=Qro#WydXkT%tynMxokt*oMCaY(vj10vErnAcPfew1bf9LB(5Ug zN7)T@>8Hy^x@@A$y>uC%%OEyjFjR4tu9N#&+-F8FcI+G4yeAWK#N+ zk&M!z*KJ>WXlVD&t%%||W_|mp6d2vM422RX+j__9e+7)MeE1cggb6^*OnVE!Tgppcln@({qnX=3v zhIBBTjm4wuV^I(#+&r3sV00#lk~X!{cFkckor$HAu7ZwrX)|t+fXq$DEeMJ>3tHw8 z%!>ySOsArTWkArJl0h>SMOiM{5_a_15}ZfHMj{qLt0~llm!L3UD+^~bfaiV^bR-0w zHN)9SeL5RP2@HH$jwL5joHxSZw0W3I*sD#>W2VE2!|z+`1^%vr^K0fKQm}qgr-I;&)+#rH7Uq)Jl=6Ri1q)8YfCK8-$P9ME67V& zERE1qj^+8giD^K*k8B@lVTJ9lmI^eagtI(+FP`pB%Xu{~l+XlNs|~`_Cih zl;`hara$w@^Ydf)CCt$JB(Ll8_cQ;Uj+dXyOn-#3v>wRv{GEMCPQjTESw!M8(;p*B zvBUB_pX9%T;*}rySPRdOpUaylOZU(6Jdd1}Q}h9>b0)USbRH|6^1S}ODCN7H!kA|{ zrvL1b=XK^*)fQwvyHu3tGR@zKhyCaI@oLrP;C^bCO67lr736=286;)-dptPtQeukd z`3rebAun51JJaI?G0$=gUn_%AU0S6p0Ij1rlUKeT0WU`UtsS=10jDtLy}YX`$WKZ6 zbshmv5xywpz3unRTHEm63ieqKeP46l$@;#O-{YzD6k)J}{KigOo0s2P9<3n1Ny>Y_ zoW1f?_BkDw`2$k^fDB-|9cl}XXO2tSSK`=}qJqq>vrGTtQSP47>v#0Z$SB E8<^D&`v3p{ literal 0 HcmV?d00001 diff --git a/common/tvm_utility/data/models/abs_model_aarch64/deploy_param.params b/common/tvm_utility/data/models/abs_model_aarch64/deploy_param.params new file mode 100644 index 0000000000000000000000000000000000000000..1011def01ed82125c7ff17bdeed9fabb523a92c7 GIT binary patch literal 32 Scmdl!hlSO@_vv>A8~^}u(F7F$ literal 0 HcmV?d00001 diff --git a/common/tvm_utility/data/models/abs_model_aarch64/inference_engine_tvm_config.hpp b/common/tvm_utility/data/models/abs_model_aarch64/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..09c8c0beacebe --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_aarch64/inference_engine_tvm_config.hpp @@ -0,0 +1,54 @@ +// Copyright 2021 Arm Limited and Contributors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tvm_utility/pipeline.hpp" + +#ifndef COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_AARCH64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_AARCH64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + +namespace model_zoo +{ +namespace inf_test +{ +namespace engine_load +{ +namespace abs_model +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {0, 0, 0}, // modelzoo_version + + // cspell: ignore mtriple + "abs_model_aarch64", // network_name + "llvm -mtriple=aarch64-linux-gnu", // network_backend + + "deploy_lib.so", // network_module_path + "deploy_graph.json", // network_graph_path + "deploy_param.params", // network_params_path + + // cspell: ignore DLCPU + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"a", kDLFloat, 32, 1, {2, 2}}}, // network_inputs + + {{"output", kDLFloat, 32, 1, {2, 2}}} // network_outputs +}; + +} // namespace abs_model +} // namespace engine_load +} // namespace inf_test +} // namespace model_zoo +#endif // COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_AARCH64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + // NOLINT diff --git a/common/tvm_utility/data/models/abs_model_x86_64/deploy_graph.json b/common/tvm_utility/data/models/abs_model_x86_64/deploy_graph.json new file mode 100644 index 0000000000000..b226c01747dca --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_x86_64/deploy_graph.json @@ -0,0 +1,36 @@ +{ + "nodes": [ + { + "op": "null", + "name": "a", + "inputs": [] + }, + { + "op": "tvm_op", + "name": "tvmgen_default_fused_abs", + "attrs": { + "num_outputs": "1", + "num_inputs": "1", + "flatten_data": "0", + "func_name": "tvmgen_default_fused_abs", + "hash": "1be44995aa501758" + }, + "inputs": [[0, 0, 0]] + } + ], + "arg_nodes": [0], + "heads": [[1, 0, 0]], + "attrs": { + "dltype": ["list_str", ["float32", "float32"]], + "device_index": ["list_int", [1, 1]], + "storage_id": ["list_int", [0, 1]], + "shape": [ + "list_shape", + [ + [2, 2], + [2, 2] + ] + ] + }, + "node_row_ptr": [0, 1, 2] +} diff --git a/common/tvm_utility/data/models/abs_model_x86_64/deploy_lib.so b/common/tvm_utility/data/models/abs_model_x86_64/deploy_lib.so new file mode 100644 index 0000000000000000000000000000000000000000..9a6d02817e048aa8030e6969f39cb4a5a50b62d2 GIT binary patch literal 18168 zcmeHP3v3+48J_dS$&rW8c|Z(l$p!^vLvr!ic{mRzcCejGutOXiutn)+eYa;H+{3-w z3wD%(+apDhv6>=80jUK+i>N>l5K=&DDQU{1rJ?~<2vpPtlt7|B6j}i;rMdq9&ir?~ zvwKcbBC4u4^6o$L{g3(Q--q}&%@i0nWww__BrJ5rxg7MsXkpa@en%D(! zUc?krc1n@u$V~_)Q6}tS9hE38$})Xk=n*A8>X|GBl_4qSxj>K1;ANs#y{W(&3UL0A zXb;f?Xc*F~f|6bZV5m1L^hSjq(QQJWs1J=mLLc#HLIW;zF^3sZl*P2|xDmnpU00e# z2VKr#MwIlrK@a^r+eBgCF6t{4m;IuBL@7L2?>soXaBSMTgT3_N(f#jy``6!k_IK~z zyC(bKV;|gm`*Y|E^6@&O9{N{5&CG5?tPrbbejZqN=%>%#zA^sxj1QiE09Ykc2XGV@ z?vfHXw$n`>{c*$Z27C_mZZ;b-klq*9Z!hE*u^DVgT$hVYE@L}Pd>*T9;Q+PiW`WmB zI8)VuL@KFf3@vS_DpNQ2bg2OES2T<1J@jv5};)MzZJ#bdic zQ0=+Ct99Gv>vW@2%NXtHbSllkjav8?JsH`irL}lmk9TU>WOxu*#;$~#&|*pO0I~ym z5`2hi*|?!bvl%_2YW*4B9vUCiu2Z7p42y9<_yK{}NbRNakqz7)_5qANrME4(2(`&N zYQeD&Q95D4rx6JBlm#cBkUe3+X{=b(%Gq)R$`L3>pd5j61j-R8N1z;mas>XbBJg+r zrJsdzfA)pODo-wBEVTDUqauGKl>4Rcuo;K^iuVD|Uw#bE{)H`&djN9L{S>6oc=J1` zH(vQBlGPK&g<$GCmgK|%f}ru>SBk<^_I?K>4Z^w$lcLlfSR=jG5F zr;T}FpjjC3<&Q@F3)_nN==NZ$ypUNG;=4=0*_gG!3E~eUOXh zhS3cjvZKT3#*JnU-ROo~Yi>2E{u~|4!M{ zm;~?R@GW!g(rFI=QG##e@OAJ|#p+2LhhLK5r5xUQpouW$dlvgb z`;5iaaEbnOl^Ga(mJ=E`7W;y4=Br`&*Bs_*p?z^%o%}PVv5~b9;!Zk`kGBcRU#v@97 zv#gHVIcM){aB^Q8FnmTV9l#g486%wy8|r{=h)^z3>X#^HT_WJgNGzemGKy{nLyuq_ z^lxOfxXVSF-0F%tthqJ|0c@&ks8g=ELaCixW2IH2u>mgPnRQkbb%HDFf_Q~ROCrX| zkS;d`E@?EL(u_vA%b8&UR~s9slVt|AAsu=GnruxEWD|PQ>>SuFiKh%|8AVGf*IF%UE%u$xsM&$ajudG~NSAXU%+vIjt`bP|1O+}*_-8f@d z%zk;+bxDqzahkj@2kYHDjOe>!VO=$Q=9wI+Q(+ysqt$F;Px&)D0fQDfJ3jKfK!T_yvov2&jf?Ai`jI5V^!51RW)1uGj8>bvJIE4 zy|QufWeB4_yk26pAZOh-b_ZZzgX1XR<{rS-s%r0gPnEB|s`~mWAC$g?Gt`g%9fKou zBA++69=25}dX>-Ip87f9I|0YIlv6nZ{|gbI{iC#BbS61%4pPQuiSiR-VADQZ+K0M8 z@M)}GBFeNM6~{a#;(vTPpThD^vAGoI7ED88Qz`A4zEAL{b2fWiP_dny)rfLoFKP=1 zXb-9~g_o(`w}m|HBsLT2|0B=+DcBMG{}!Qr>~j6IZ+FwxSHlg@j{a=Y$SSJ?%LDaG zgITi_yse==P`^A7tm8O?7m18V@lnV%z)|XPfmzwZaOOet!F-LXB7_Xy78myjTlQgB zST|*RZonRL9~@>V8zJ)`79N9B6C4j=0e9s$L&g-FfWk&7m|_9&bQ2UFg2Ve36gmZm zO@SB4ta3`_R8(S~*`CX*=2m*Es^>4LdUoT*Pj8$y+cO7xO+^4$bEdY`c&j$g-2mOx zTgfWsV*#4An0c$#&zJ$7rxM;;v#EZDFnbSSOOl3#PNO} z07w@UT048XGId=vu+A5Szb(;TIJW$7U381@*t!4^4u~_B)&=lviba)P!wB>$%9nDk zc+CSXg01M5wk$IeL(58c1oO6(!M%MDPYrl##M5#-t-#Yt9u7F)Epi^VqYa}5X}h2R z;Wh_Mu_Aw z$j@7f5c&D-B1C@PUxdid14W4Ze5DAHpMNMqP7gUUR`9di>1Vgo&u*um-A+Hdoql#Z z{p@!7*~s3r=k&AB>1UtQ&pwBrHGshU2nnZkoD}(>MP%~iWuC(KPgY4U*xSiX zT49*Ty^MZ`tqd7r!(T>j?`C)~7MW75A}vR~)`_bH%4KTA$eM z!-TI`5uW$5nT7ATtXwawc3w0W^;Q&qN#GZd);m^?m-!j3bL@CEbNe0l!tW)0cVm}_ zok}fMgy+3%E~DRlE7uEmA&RR!>@#vnUUrcSitU?h)OkPH-KReIMXiAWAS{=$b ztqNuQt9AfNIBia0ALV5Ru2U=Ut4FCn;93RoAqih3a9Zb*y;YF$SmG(BY1U!l&g+Y- z0knTgvOi+VJNpTaPdEQ@(PP@dbqFNtcBR^T2xL6aKYxR(Iw`r?|1HRPV9?3^@NE;H z>*znn1TOctj{vW58NZ(b&RoXPbZ9^YbB@Pqz&-FKpWM%{09-L`*-tA<;MW4~_B|l( z7 zA~6CgmXZpRhD{V!eO;+Y$%USZ4rJj$KG^Bl-rCi!wr_32&mY_RwzhU{z8YZo^pQX{ zl1Zt9S~7wkL1IGl-VL0Qp1))?#Gc*>^2gTJKjDRYY zxGKM_T!ZppS4+ggP=Crm%TTRpr$3Wn@TxR{Z_xnL`?CY;P_`e0p(l!REE!EvUhD5q z>$|8Fk0o`=Lz^iZPeDc8RzeF~)xk8rWXbTM@`cX$OM&<9v<#8I(_SoDmzNm<;C}-5 z@}$30WQZ;o`gEU9AsH3&j#fdg$*{j@$J`Z(~c z32j(Y2nuMM^cB%SqEx@^Kh=-xZnOz2EuQH2Nl^KhSdQN_pk>vkbs14ISHe8&mGo)- zMpTYBm5FMSKK-r}jY|&8{tQU^v>qf%&#}mV$sEJqb!C-y-$THM||5Lz8;Y|8O zp9GFo-yD!AEfi^-kO9&odQj4*bx@7aCplT4%0Gh)%8>s`3+E_jsGmzQ_6x|M|8hG? zUtac!aGE-3<29j=d26~x7!+YFut=YP-$O>mkhfdtKT3jv%Ca8t z`!R52422Uy|9C-@XJlD?Zikp0zwHfNUVfTE`b1|6eW@Duh|s@{F02qBJ(8baLVuBX zUM9=SWn2f#7}~x}=*xFZvMiPha~C-Uv_MeN!Sso WLaAsz33k&T#mBvnR!Ry|k^LLHo-jB7 literal 0 HcmV?d00001 diff --git a/common/tvm_utility/data/models/abs_model_x86_64/deploy_param.params b/common/tvm_utility/data/models/abs_model_x86_64/deploy_param.params new file mode 100644 index 0000000000000000000000000000000000000000..1011def01ed82125c7ff17bdeed9fabb523a92c7 GIT binary patch literal 32 Scmdl!hlSO@_vv>A8~^}u(F7F$ literal 0 HcmV?d00001 diff --git a/common/tvm_utility/data/models/abs_model_x86_64/inference_engine_tvm_config.hpp b/common/tvm_utility/data/models/abs_model_x86_64/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..7a7e3ef97c1b3 --- /dev/null +++ b/common/tvm_utility/data/models/abs_model_x86_64/inference_engine_tvm_config.hpp @@ -0,0 +1,53 @@ +// Copyright 2021 Arm Limited and Contributors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tvm_utility/pipeline.hpp" + +#ifndef COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_X86_64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_X86_64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + +namespace model_zoo +{ +namespace inf_test +{ +namespace engine_load +{ +namespace abs_model +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {0, 0, 0}, // modelzoo_version + + "abs_model_x86_64", // network_name + "llvm", // network_backend + + "deploy_lib.so", // network_module_path + "deploy_graph.json", // network_graph_path + "deploy_param.params", // network_params_path + + // cspell: ignore DLCPU + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"a", kDLFloat, 32, 1, {2, 2}}}, // network_inputs + + {{"output", kDLFloat, 32, 1, {2, 2}}} // network_outputs +}; + +} // namespace abs_model +} // namespace engine_load +} // namespace inf_test +} // namespace model_zoo +#endif // COMMON__TVM_UTILITY__DATA__MODELS__ABS_MODEL_X86_64__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + // NOLINT diff --git a/common/tvm_utility/data/models/yolo_v2_tiny/inference_engine_tvm_config.hpp b/common/tvm_utility/data/models/yolo_v2_tiny/inference_engine_tvm_config.hpp new file mode 100644 index 0000000000000..45ff0d8ce33e3 --- /dev/null +++ b/common/tvm_utility/data/models/yolo_v2_tiny/inference_engine_tvm_config.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 Arm Limited and Contributors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "tvm_utility/pipeline.hpp" + +#ifndef COMMON__TVM_UTILITY__DATA__MODELS__YOLO_V2_TINY__INFERENCE_ENGINE_TVM_CONFIG_HPP_ // NOLINT +#define COMMON__TVM_UTILITY__DATA__MODELS__YOLO_V2_TINY__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + +namespace model_zoo +{ +namespace perception +{ +namespace camera_obstacle_detection +{ +namespace yolo_v2_tiny +{ +namespace tensorflow_fp32_coco +{ + +static const tvm_utility::pipeline::InferenceEngineTVMConfig config{ + {3, 0, 0}, // modelzoo_version + + "yolo_v2_tiny", // network_name + "llvm", // network_backend + + // cspell: ignore DLCPU + "./deploy_lib.so", // network_module_path + "./deploy_graph.json", // network_graph_path + "./deploy_param.params", // network_params_path + + kDLCPU, // tvm_device_type + 0, // tvm_device_id + + {{"input", kDLFloat, 32, 1, {-1, 416, 416, 3}}}, // network_inputs + + {{"output", kDLFloat, 32, 1, {1, 13, 13, 425}}} // network_outputs +}; + +} // namespace tensorflow_fp32_coco +} // namespace yolo_v2_tiny +} // namespace camera_obstacle_detection +} // namespace perception +} // namespace model_zoo +#endif // COMMON__TVM_UTILITY__DATA__MODELS__YOLO_V2_TINY__INFERENCE_ENGINE_TVM_CONFIG_HPP_ + // NOLINT diff --git a/common/tvm_utility/test/yolo_v2_tiny/main.cpp b/common/tvm_utility/example/yolo_v2_tiny/main.cpp similarity index 80% rename from common/tvm_utility/test/yolo_v2_tiny/main.cpp rename to common/tvm_utility/example/yolo_v2_tiny/main.cpp index aac7900f423c2..8a89436170894 100644 --- a/common/tvm_utility/test/yolo_v2_tiny/main.cpp +++ b/common/tvm_utility/example/yolo_v2_tiny/main.cpp @@ -12,30 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "gtest/gtest.h" #include "tvm_utility/pipeline.hpp" #include "yolo_v2_tiny/inference_engine_tvm_config.hpp" #include +#include #include +#include +#include #include #include #include using model_zoo::perception::camera_obstacle_detection::yolo_v2_tiny::tensorflow_fp32_coco::config; -// Name of file containing the human readable names of the classes. One class -// on each line. -static constexpr const char * LABEL_FILENAME = "./yolo_v2_tiny_artifacts/labels.txt"; - -// Name of file containing the anchor values for the network. Each line is one -// anchor. each anchor has 2 comma separated floating point values. -static constexpr const char * ANCHOR_FILENAME = "./yolo_v2_tiny_artifacts/anchors.csv"; - -// Filename of the image on which to run the inference -static constexpr const char * IMAGE_FILENAME = "./yolo_v2_tiny_artifacts/test_image_0.jpg"; - namespace tvm_utility { namespace yolo_v2_tiny @@ -118,16 +109,18 @@ class PreProcessorYoloV2Tiny : public tvm_utility::pipeline::PreProcessor> { public: - explicit PostProcessorYoloV2Tiny(tvm_utility::pipeline::InferenceEngineTVMConfig config) + explicit PostProcessorYoloV2Tiny( + tvm_utility::pipeline::InferenceEngineTVMConfig config, std::string label_filename, + std::string anchor_filename) : network_output_width(config.network_outputs[0].node_shape[1]), network_output_height(config.network_outputs[0].node_shape[2]), network_output_depth(config.network_outputs[0].node_shape[3]), network_output_datatype_bytes(config.network_outputs[0].tvm_dtype_bits / 8) { // Parse human readable names for the classes - std::ifstream label_file{LABEL_FILENAME}; + std::ifstream label_file{label_filename}; if (!label_file.good()) { - std::string label_filename = LABEL_FILENAME; + std::string label_filename = label_filename; throw std::runtime_error("unable to open label file:" + label_filename); } std::string line{}; @@ -136,9 +129,9 @@ class PostProcessorYoloV2Tiny : public tvm_utility::pipeline::PostProcessor> anchors{}; }; -TEST(PipelineExamples, SimplePipeline) +} // namespace yolo_v2_tiny +} // namespace tvm_utility + +bool check_near(double expected, double actual, double tolerance) +{ + return fabs(expected - actual) <= tolerance; +} + +int main(int argc, char * argv[]) { + // init node to use parameters + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("yolo_v2_tiny_example"); + // Filename of the image on which to run the inference + node->declare_parameter("image_filename"); + // Name of file containing the human readable names of the classes. One class + // on each line. + node->declare_parameter("label_filename"); + // Name of file containing the anchor values for the network. Each line is one + // anchor. each anchor has 2 comma separated floating point values. + node->declare_parameter("anchor_filename"); + // Packages data and artifacts directory path. + node->declare_parameter("data_path"); + + RCLCPP_INFO(node->get_logger(), "Node started"); + // Instantiate the pipeline - using PrePT = PreProcessorYoloV2Tiny; + using PrePT = tvm_utility::yolo_v2_tiny::PreProcessorYoloV2Tiny; using IET = tvm_utility::pipeline::InferenceEngineTVM; - using PostPT = PostProcessorYoloV2Tiny; + using PostPT = tvm_utility::yolo_v2_tiny::PostProcessorYoloV2Tiny; PrePT PreP{config}; - IET IE{config, "tvm_utility"}; - PostPT PostP{config}; + IET IE{config, "tvm_utility", node->get_parameter("data_path").as_string()}; + PostPT PostP{ + config, node->get_parameter("label_filename").as_string(), + node->get_parameter("anchor_filename").as_string()}; tvm_utility::pipeline::Pipeline pipeline(PreP, IE, PostP); - auto version_status = IE.version_check({2, 0, 0}); - EXPECT_NE(version_status, tvm_utility::Version::Unsupported); - // Push data input the pipeline and get the output - auto output = pipeline.schedule(IMAGE_FILENAME); + auto output = pipeline.schedule(node->get_parameter("image_filename").as_string()); // Define reference vector containing expected values, expressed as hexadecimal integers std::vector int_output{0x3eb64594, 0x3f435656, 0x3ece1600, 0x3e99d381, @@ -271,11 +287,21 @@ TEST(PipelineExamples, SimplePipeline) } // Test: check if the generated output is equal to the reference - EXPECT_EQ(expected_output.size(), output.size()) << "Unexpected output size"; + if (expected_output.size() == output.size()) { + RCLCPP_INFO(node->get_logger(), "Model has proper output size"); + } else { + RCLCPP_INFO(node->get_logger(), "Model has unexpected output size"); + } + for (size_t i = 0; i < output.size(); ++i) { - EXPECT_NEAR(expected_output[i], output[i], 0.0001) << "at index: " << i; + if (check_near(expected_output[i], output[i], 0.0001)) { + std::cout << "Model has proper output at index: " << i << std::endl; + RCLCPP_INFO(node->get_logger(), "Model has proper output at index: %zu", i); + + } else { + RCLCPP_INFO(node->get_logger(), "Model has unexpected output at index: %zu", i); + } } + rclcpp::shutdown(); + return 0; } - -} // namespace yolo_v2_tiny -} // namespace tvm_utility diff --git a/common/tvm_utility/include/tvm_utility/pipeline.hpp b/common/tvm_utility/include/tvm_utility/pipeline.hpp index 8504da193214f..a053d5ee471be 100644 --- a/common/tvm_utility/include/tvm_utility/pipeline.hpp +++ b/common/tvm_utility/include/tvm_utility/pipeline.hpp @@ -224,12 +224,19 @@ typedef struct class InferenceEngineTVM : public InferenceEngine { public: - explicit InferenceEngineTVM(const InferenceEngineTVMConfig & config, const std::string & pkg_name) + explicit InferenceEngineTVM( + const InferenceEngineTVMConfig & config, const std::string & pkg_name, + const std::string & autoware_data_path = "") : config_(config) { // Get full network path - std::string network_prefix = ament_index_cpp::get_package_share_directory(pkg_name) + - "/models/" + config.network_name + "/"; + std::string network_prefix; + if (autoware_data_path == "") { + network_prefix = ament_index_cpp::get_package_share_directory(pkg_name) + "/models/" + + config.network_name + "/"; + } else { + network_prefix = autoware_data_path + "/" + pkg_name + "/models/" + config.network_name + "/"; + } std::string network_module_path = network_prefix + config.network_module_path; std::string network_graph_path = network_prefix + config.network_graph_path; std::string network_params_path = network_prefix + config.network_params_path; diff --git a/common/tvm_utility/launch/yolo_v2_tiny_example.launch.xml b/common/tvm_utility/launch/yolo_v2_tiny_example.launch.xml new file mode 100644 index 0000000000000..045a6fc9dfa27 --- /dev/null +++ b/common/tvm_utility/launch/yolo_v2_tiny_example.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + diff --git a/common/tvm_utility/schema/yolo_v2_tiny_example.schema.json b/common/tvm_utility/schema/yolo_v2_tiny_example.schema.json new file mode 100644 index 0000000000000..8ee1987f73a62 --- /dev/null +++ b/common/tvm_utility/schema/yolo_v2_tiny_example.schema.json @@ -0,0 +1,47 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for yolo_v2_tiny_example of tvm_utility", + "type": "object", + "definitions": { + "yolo_v2_tiny_example": { + "type": "object", + "properties": { + "image_filename": { + "type": "string", + "default": "$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/test_image_0.jpg", + "description": "Filename of the image on which to run the inference." + }, + "label_filename": { + "type": "string", + "default": "$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/labels.txt", + "description": "Name of file containing the human readable names of the classes. One class on each line." + }, + "anchor_filename": { + "type": "string", + "default": "$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/anchors.csv", + "description": "Name of file containing the anchor values for the network. Each line is one anchor. each anchor has 2 comma separated floating point values." + }, + "data_path": { + "type": "string", + "default": "$(env HOME)/autoware_data", + "description": "Packages data and artifacts directory path." + } + }, + "required": ["image_filename", "label_filename", "anchor_filename", "data_path"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/yolo_v2_tiny_example" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/common/tvm_utility/test/abs_model/main.cpp b/common/tvm_utility/test/abs_model/main.cpp new file mode 100644 index 0000000000000..4bf1a69c0556b --- /dev/null +++ b/common/tvm_utility/test/abs_model/main.cpp @@ -0,0 +1,146 @@ +// Copyright 2021-2022 Arm Limited and Contributors. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gtest/gtest.h" +#include "tvm_utility/pipeline.hpp" +// file for current arch x86 or arm is chosen in cmake file +#include +#include + +#include +#include +#include +#include +#include + +using model_zoo::inf_test::engine_load::abs_model::config; + +namespace tvm_utility +{ +namespace abs_model +{ + +class PreProcessorLinearModel : public tvm_utility::pipeline::PreProcessor> +{ +public: + explicit PreProcessorLinearModel(tvm_utility::pipeline::InferenceEngineTVMConfig config) + : network_input_a_width(config.network_inputs[0].node_shape[0]), + network_input_a_height(config.network_inputs[0].node_shape[1]), + network_input_datatype_bytes(config.network_inputs[0].tvm_dtype_bits / 8) + { + // Allocate input variable + std::vector shape_a{network_input_a_width, network_input_a_height}; + tvm_utility::pipeline::TVMArrayContainer a{ + shape_a, + config.network_inputs[0].tvm_dtype_code, + config.network_inputs[0].tvm_dtype_bits, + config.network_inputs[0].tvm_dtype_lanes, + config.tvm_device_type, + config.tvm_device_id}; + + output = a; + } + + // The cv::Mat can't be used as an input because it throws an exception when + // passed as a constant reference + tvm_utility::pipeline::TVMArrayContainerVector schedule(const std::vector & input) + { + float input_mat[2][2]; + input_mat[0][0] = input[0]; + input_mat[0][1] = input[1]; + input_mat[1][0] = input[2]; + input_mat[1][1] = input[3]; + + // Create cv::Mat from input array + cv::Mat a_input = cv::Mat(2, 2, CV_32F, &input_mat); + + TVMArrayCopyFromBytes( + output.getArray(), a_input.data, + network_input_a_width * network_input_a_height * network_input_datatype_bytes); + + return {output}; + } + +private: + int64_t network_input_a_width; + int64_t network_input_a_height; + int64_t network_input_datatype_bytes; + tvm_utility::pipeline::TVMArrayContainer output; +}; + +class PostProcessorLinearModel : public tvm_utility::pipeline::PostProcessor> +{ +public: + explicit PostProcessorLinearModel(tvm_utility::pipeline::InferenceEngineTVMConfig config) + : network_output_width(config.network_outputs[0].node_shape[0]), + network_output_height(config.network_outputs[0].node_shape[1]), + network_output_datatype_bytes(config.network_outputs[0].tvm_dtype_bits / 8) + { + } + + std::vector schedule(const tvm_utility::pipeline::TVMArrayContainerVector & input) + { + // Assert data is stored row-majored in input and the dtype is float + assert(input[0].getArray()->strides == nullptr); + assert(input[0].getArray()->dtype.bits == sizeof(float) * 8); + + // Copy the inference data to CPU memory + std::vector infer(network_output_width * network_output_height, 0.0f); + + TVMArrayCopyToBytes( + input[0].getArray(), infer.data(), + network_output_width * network_output_height * network_output_datatype_bytes); + + return infer; + } + +private: + int64_t network_output_width; + int64_t network_output_height; + int64_t network_output_datatype_bytes; +}; + +TEST(PipelineExamples, SimplePipeline) +{ + // // Instantiate the pipeline + using PrePT = PreProcessorLinearModel; + using IET = tvm_utility::pipeline::InferenceEngineTVM; + using PostPT = PostProcessorLinearModel; + + PrePT PreP{config}; + IET IE{config, "tvm_utility"}; + PostPT PostP{config}; + + tvm_utility::pipeline::Pipeline pipeline(PreP, IE, PostP); + + auto version_status = IE.version_check({2, 0, 0}); + EXPECT_NE(version_status, tvm_utility::Version::Unsupported); + + // create input array + std::vector input_arr{-1., -2., -3., 4.}; + // send it to the model + auto output = pipeline.schedule(input_arr); + + // define vector with expected values + std::vector expected_output{1., 2., 3., 4.}; + + // // Test: check if the generated output is equal to the reference + EXPECT_EQ(expected_output.size(), output.size()) << "Unexpected output size"; + for (size_t i = 0; i < output.size(); ++i) { + EXPECT_NEAR(expected_output[i], output[i], 0.0001) << "at index: " << i; + } +} + +} // namespace abs_model +} // namespace tvm_utility diff --git a/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md b/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md index 188918a8f74df..39bcc640c2147 100644 --- a/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md +++ b/common/tvm_utility/tvm-utility-yolo-v2-tiny-tests.md @@ -7,20 +7,41 @@ output. ## Compiling the Example -1. Download an example image to be used as test input. this image needs to be - saved in the `artifacts/yolo_v2_tiny/` folder + -```sh -curl https://raw.githubusercontent.com/pjreddie/darknet/master/data/dog.jpg \ - > artifacts/yolo_v2_tiny/test_image_0.jpg -``` +1. Check if model was downloaded during the env preparation step by ansible and + models files exist in the folder $HOME/autoware_data/tvm_utility/models/yolo_v2_tiny. -1. Build and test with the `DOWNLOAD_ARTIFACTS` flag set. + If not you can download them manually, see [Manual Artifacts Downloading](https://github.com/autowarefoundation/autoware/tree/main/ansible/roles/artifacts). -```sh -colcon build --packages-up-to tvm_utility --cmake-args -DDOWNLOAD_ARTIFACTS=ON -colcon test --packages-select tvm_utility -``` +2. Download an example image to be used as test input. This image needs to be + saved in the `artifacts/yolo_v2_tiny/` folder. + + ```sh + curl https://raw.githubusercontent.com/pjreddie/darknet/master/data/dog.jpg \ + > artifacts/yolo_v2_tiny/test_image_0.jpg + ``` + +3. Build. + + ```sh + colcon build --packages-up-to tvm_utility --cmake-args -DBUILD_EXAMPLE=ON + ``` + +4. Run. + + ```sh + ros2 launch tvm_utility yolo_v2_tiny_example.launch.xml + ``` + +## Parameters + +| Name | Type | Default Value | Description | +| ----------------- | ------ | ----------------------------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------- | +| `image_filename` | string | `$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/test_image_0.jpg` | Filename of the image on which to run the inference. | +| `label_filename` | string | `$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/labels.txt` | Name of file containing the human readable names of the classes. One class on each line. | +| `anchor_filename` | string | `$(find-pkg-share tvm_utility)/artifacts/yolo_v2_tiny/anchors.csv` | Name of file containing the anchor values for the network. Each line is one anchor. each anchor has 2 comma separated floating point values. | +| `data_path` | string | `$(env HOME)/autoware_data` | Packages data and artifacts directory path. | ## GPU backend @@ -28,5 +49,5 @@ Vulkan is supported by default by the tvm_vendor package. It can be selected by setting the `tvm_utility_BACKEND` variable: ```sh -colcon build --packages-up-to tvm_utility --cmake-args -DDOWNLOAD_ARTIFACTS=ON -Dtvm_utility_BACKEND=vulkan +colcon build --packages-up-to tvm_utility -Dtvm_utility_BACKEND=vulkan ``` diff --git a/common/tvm_utility/tvm_utility-extras.cmake b/common/tvm_utility/tvm_utility-extras.cmake index 4214aa4995f0f..414644c1fe041 100644 --- a/common/tvm_utility/tvm_utility-extras.cmake +++ b/common/tvm_utility/tvm_utility-extras.cmake @@ -12,12 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -# Get user-provided variables -set(DOWNLOAD_ARTIFACTS OFF CACHE BOOL "enable artifacts download") -set(MODELZOO_VERSION "3.0.0-20221221" CACHE STRING "targeted ModelZoo version") - # -# Download the selected neural network if it is not already present on disk. # Make inference_engine_tvm_config.hpp available under "data/models/${MODEL_NAME}/". # Install the TVM artifacts to "share/${PROJECT_NAME}/models/". # Return the name of the custom target in the DEPENDENCY parameter. @@ -34,48 +29,16 @@ function(get_neural_network MODEL_NAME MODEL_BACKEND DEPENDENCY) set(EXTERNALPROJECT_NAME ${MODEL_NAME}_${MODEL_BACKEND}) set(PREPROCESSING "") - # Prioritize user-provided models. - # cspell: ignore COPYONLY - if(IS_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}") - message(STATUS "Using user-provided model from ${DATA_PATH}/user/${MODEL_NAME}") - file(REMOVE_RECURSE "${DATA_PATH}/models/${MODEL_NAME}/") - configure_file( - "${DATA_PATH}/user/${MODEL_NAME}/inference_engine_tvm_config.hpp" - "${DATA_PATH}/models/${MODEL_NAME}/inference_engine_tvm_config.hpp" - COPYONLY - ) - if(EXISTS "${DATA_PATH}/user/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") - configure_file( - "${DATA_PATH}/user/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp" - "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp" - COPYONLY - ) - endif() - set(SOURCE_DIR "${DATA_PATH}/user/${MODEL_NAME}") - set(INSTALL_DIRECTORY "${DATA_PATH}/user/${MODEL_NAME}") - else() - set(ARCHIVE_NAME "${MODEL_NAME}-${CMAKE_SYSTEM_PROCESSOR}-${MODEL_BACKEND}-${MODELZOO_VERSION}.tar.gz") - - # Use previously-downloaded archives if available. - set(DOWNLOAD_DIR "${DATA_PATH}/downloads") - if(DOWNLOAD_ARTIFACTS) - message(STATUS "Downloading ${ARCHIVE_NAME} ...") - if(NOT EXISTS "${DATA_PATH}/downloads/${ARCHIVE_NAME}") - set(URL "https://autoware-modelzoo.s3.us-east-2.amazonaws.com/models/${MODELZOO_VERSION}/${ARCHIVE_NAME}") - file(DOWNLOAD ${URL} "${DOWNLOAD_DIR}/${ARCHIVE_NAME}") - endif() - else() - message(WARNING "Skipped download for ${MODEL_NAME} (enable by setting DOWNLOAD_ARTIFACTS)") - set(${DEPENDENCY} "" PARENT_SCOPE) - return() - endif() + if(IS_DIRECTORY "${DATA_PATH}/models/${MODEL_NAME}") set(SOURCE_DIR "${DATA_PATH}/models/${MODEL_NAME}") set(INSTALL_DIRECTORY "${DATA_PATH}/models/${MODEL_NAME}") - file(ARCHIVE_EXTRACT INPUT "${DOWNLOAD_DIR}/${ARCHIVE_NAME}" DESTINATION "${SOURCE_DIR}") if(EXISTS "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") set(PREPROCESSING "${DATA_PATH}/models/${MODEL_NAME}/preprocessing_inference_engine_tvm_config.hpp") endif() - + else() + message(WARNING "No model configuration files were provided") + set(${DEPENDENCY} "" PARENT_SCOPE) + return() endif() include(ExternalProject) From bfabbcaf9a87683e1bebc04ecf54c68eea567bec Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 27 Oct 2023 09:52:14 +0900 Subject: [PATCH 188/206] fix(ekf_localizer): remove unnecessary publishers (#5423) Signed-off-by: kminoda --- .../include/ekf_localizer/ekf_localizer.hpp | 4 --- .../ekf_localizer/src/ekf_localizer.cpp | 25 ------------------- 2 files changed, 29 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 4fc2305cc7adc..3a7c2eb4b9f22 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -117,10 +117,6 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_twist_; //!< @brief ekf estimated twist with covariance publisher rclcpp::Publisher::SharedPtr pub_twist_cov_; - //!< @brief debug info publisher - rclcpp::Publisher::SharedPtr pub_debug_; - //!< @brief debug measurement pose publisher - rclcpp::Publisher::SharedPtr pub_measured_pose_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index b39112b1d8d62..6fea079da8ea6 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -109,10 +109,6 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti z_filter_.set_proc_dev(1.0); roll_filter_.set_proc_dev(0.01); pitch_filter_.set_proc_dev(0.01); - - /* debug */ - pub_debug_ = create_publisher("debug", 1); - pub_measured_pose_ = create_publisher("debug/measured_pose", 1); } /* @@ -637,27 +633,6 @@ void EKFLocalizer::publishEstimateResult() odometry.pose = pose_cov.pose; odometry.twist = twist_cov.twist; pub_odom_->publish(odometry); - - /* debug measured pose */ - if (!pose_queue_.empty()) { - geometry_msgs::msg::PoseStamped p; - p.pose = pose_queue_.back()->pose.pose; - p.header.stamp = current_time; - pub_measured_pose_->publish(p); - } - - /* debug publish */ - double pose_yaw = 0.0; - if (!pose_queue_.empty()) { - pose_yaw = tf2::getYaw(pose_queue_.back()->pose.pose.orientation); - } - - tier4_debug_msgs::msg::Float64MultiArrayStamped msg; - msg.stamp = current_time; - msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAW))); // [0] ekf yaw angle - msg.data.push_back(tier4_autoware_utils::rad2deg(pose_yaw)); // [1] measurement yaw angle - msg.data.push_back(tier4_autoware_utils::rad2deg(X(IDX::YAWB))); // [2] yaw bias - pub_debug_->publish(msg); } void EKFLocalizer::publishDiagnostics() From 025f2cd7101fd1200aaabb930a8c3590f1212c31 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 27 Oct 2023 14:50:38 +0900 Subject: [PATCH 189/206] refactor(ekf_localizer): remove current_ekf_pose and so on (#5422) * refactor(ekf_localizer): remote current_ekf_pose and so on Signed-off-by: kminoda * still build failes due to one current_ekf_twist_ Signed-off-by: kminoda * now works Signed-off-by: kminoda * style(pre-commit): autofix --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/kalman_filter/kalman_filter.hpp | 6 +- common/kalman_filter/src/kalman_filter.cpp | 6 +- .../include/ekf_localizer/ekf_localizer.hpp | 24 ++++-- .../ekf_localizer/src/ekf_localizer.cpp | 83 +++++++++++-------- 4 files changed, 71 insertions(+), 48 deletions(-) diff --git a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp index 79a47bde3a1b2..b500cffb92279 100644 --- a/common/kalman_filter/include/kalman_filter/kalman_filter.hpp +++ b/common/kalman_filter/include/kalman_filter/kalman_filter.hpp @@ -109,20 +109,20 @@ class KalmanFilter * @brief get current kalman filter state * @param x kalman filter state */ - void getX(Eigen::MatrixXd & x); + void getX(Eigen::MatrixXd & x) const; /** * @brief get current kalman filter covariance * @param P kalman filter covariance */ - void getP(Eigen::MatrixXd & P); + void getP(Eigen::MatrixXd & P) const; /** * @brief get component of current kalman filter state * @param i index of kalman filter state * @return value of i's component of the kalman filter state x[i] */ - double getXelement(unsigned int i); + double getXelement(unsigned int i) const; /** * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. diff --git a/common/kalman_filter/src/kalman_filter.cpp b/common/kalman_filter/src/kalman_filter.cpp index 937819ffb0275..450d40936db2e 100644 --- a/common/kalman_filter/src/kalman_filter.cpp +++ b/common/kalman_filter/src/kalman_filter.cpp @@ -77,15 +77,15 @@ void KalmanFilter::setR(const Eigen::MatrixXd & R) { R_ = R; } -void KalmanFilter::getX(Eigen::MatrixXd & x) +void KalmanFilter::getX(Eigen::MatrixXd & x) const { x = x_; } -void KalmanFilter::getP(Eigen::MatrixXd & P) +void KalmanFilter::getP(Eigen::MatrixXd & P) const { P = P_; } -double KalmanFilter::getXelement(unsigned int i) +double KalmanFilter::getXelement(unsigned int i) const { return x_(i); } diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 3a7c2eb4b9f22..8da982b8c0dfa 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -89,7 +89,7 @@ class Simple1DFilter return; }; void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } - double get_x() { return x_; } + double get_x() const { return x_; } private: bool initialized_; @@ -186,13 +186,6 @@ class EKFLocalizer : public rclcpp::Node AgedObjectQueue pose_queue_; AgedObjectQueue twist_queue_; - geometry_msgs::msg::PoseStamped current_ekf_pose_; //!< @brief current estimated pose - geometry_msgs::msg::PoseStamped - current_biased_ekf_pose_; //!< @brief current estimated pose without yaw bias correction - geometry_msgs::msg::TwistStamped current_ekf_twist_; //!< @brief current estimated twist - std::array current_pose_covariance_; - std::array current_twist_covariance_; - /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ @@ -257,10 +250,23 @@ class EKFLocalizer : public rclcpp::Node */ void setCurrentResult(); + /** + * @brief get current ekf pose + */ + geometry_msgs::msg::PoseStamped getCurrentEKFPose(bool get_biased_yaw) const; + + /** + * @brief get current ekf twist + */ + geometry_msgs::msg::TwistStamped getCurrentEKFTwist() const; + /** * @brief publish current EKF estimation result */ - void publishEstimateResult(); + void publishEstimateResult( + const geometry_msgs::msg::PoseStamped & current_ekf_pose, + const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, + const geometry_msgs::msg::TwistStamped & current_ekf_twist); /** * @brief publish diagnostics message diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 6fea079da8ea6..b05ec80627570 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -232,6 +232,17 @@ void EKFLocalizer::timerCallback() } twist_no_update_count_ = twist_is_updated ? 0 : (twist_no_update_count_ + 1); + const geometry_msgs::msg::PoseStamped current_ekf_pose = getCurrentEKFPose(false); + const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = getCurrentEKFPose(true); + const geometry_msgs::msg::TwistStamped current_ekf_twist = getCurrentEKFTwist(); + + /* publish ekf result */ + publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); + publishDiagnostics(); +} + +geometry_msgs::msg::PoseStamped EKFLocalizer::getCurrentEKFPose(bool get_biased_yaw) const +{ const double x = ekf_.getXelement(IDX::X); const double y = ekf_.getXelement(IDX::Y); const double z = z_filter_.get_x(); @@ -242,27 +253,32 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const double yaw = biased_yaw + yaw_bias; - const double vx = ekf_.getXelement(IDX::VX); - const double wz = ekf_.getXelement(IDX::WZ); - - current_ekf_pose_.header.frame_id = params_.pose_frame_id; - current_ekf_pose_.header.stamp = this->now(); - current_ekf_pose_.pose.position = tier4_autoware_utils::createPoint(x, y, z); - current_ekf_pose_.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); - current_biased_ekf_pose_ = current_ekf_pose_; - current_biased_ekf_pose_.pose.orientation = - tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); + geometry_msgs::msg::PoseStamped current_ekf_pose; + current_ekf_pose.header.frame_id = params_.pose_frame_id; + current_ekf_pose.header.stamp = this->now(); + current_ekf_pose.pose.position = tier4_autoware_utils::createPoint(x, y, z); + if (get_biased_yaw) { + current_ekf_pose.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, biased_yaw); + } else { + current_ekf_pose.pose.orientation = + tier4_autoware_utils::createQuaternionFromRPY(roll, pitch, yaw); + } + return current_ekf_pose; +} - current_ekf_twist_.header.frame_id = "base_link"; - current_ekf_twist_.header.stamp = this->now(); - current_ekf_twist_.twist.linear.x = vx; - current_ekf_twist_.twist.angular.z = wz; +geometry_msgs::msg::TwistStamped EKFLocalizer::getCurrentEKFTwist() const +{ + const double vx = ekf_.getXelement(IDX::VX); + const double wz = ekf_.getXelement(IDX::WZ); - /* publish ekf result */ - publishEstimateResult(); - publishDiagnostics(); + geometry_msgs::msg::TwistStamped current_ekf_twist; + current_ekf_twist.header.frame_id = "base_link"; + current_ekf_twist.header.stamp = this->now(); + current_ekf_twist.twist.linear.x = vx; + current_ekf_twist.twist.angular.z = wz; + return current_ekf_twist; } void EKFLocalizer::showCurrentX() @@ -282,12 +298,12 @@ void EKFLocalizer::timerTFCallback() return; } - if (current_ekf_pose_.header.frame_id == "") { + if (params_.pose_frame_id == "") { return; } geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped = tier4_autoware_utils::pose2transform(current_ekf_pose_, "base_link"); + transform_stamped = tier4_autoware_utils::pose2transform(getCurrentEKFPose(false), "base_link"); transform_stamped.header.stamp = this->now(); tf_br_->sendTransform(transform_stamped); } @@ -338,8 +354,6 @@ void EKFLocalizer::callbackInitialPose( X(IDX::X) = initialpose->pose.pose.position.x + transform.transform.translation.x; X(IDX::Y) = initialpose->pose.pose.position.y + transform.transform.translation.y; - current_ekf_pose_.pose.position.z = - initialpose->pose.pose.position.z + transform.transform.translation.z; X(IDX::YAW) = tf2::getYaw(initialpose->pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); X(IDX::YAWB) = 0.0; @@ -488,7 +502,7 @@ bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar // Considering change of z value due to measurement pose delay const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); - const double dz_delay = current_ekf_twist_.twist.linear.x * delay_time * std::sin(-rpy.y); + const double dz_delay = ekf_.getXelement(IDX::VX) * delay_time * std::sin(-rpy.y); geometry_msgs::msg::PoseWithCovarianceStamped pose_with_z_delay; pose_with_z_delay = pose; pose_with_z_delay.pose.pose.position.z += dz_delay; @@ -586,36 +600,39 @@ bool EKFLocalizer::measurementUpdateTwist( /* * publishEstimateResult */ -void EKFLocalizer::publishEstimateResult() +void EKFLocalizer::publishEstimateResult( + const geometry_msgs::msg::PoseStamped & current_ekf_pose, + const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, + const geometry_msgs::msg::TwistStamped & current_ekf_twist) { rclcpp::Time current_time = this->now(); const Eigen::MatrixXd X = ekf_.getLatestX(); const Eigen::MatrixXd P = ekf_.getLatestP(); /* publish latest pose */ - pub_pose_->publish(current_ekf_pose_); - pub_biased_pose_->publish(current_biased_ekf_pose_); + pub_pose_->publish(current_ekf_pose); + pub_biased_pose_->publish(current_biased_ekf_pose); /* publish latest pose with covariance */ geometry_msgs::msg::PoseWithCovarianceStamped pose_cov; pose_cov.header.stamp = current_time; - pose_cov.header.frame_id = current_ekf_pose_.header.frame_id; - pose_cov.pose.pose = current_ekf_pose_.pose; + pose_cov.header.frame_id = current_ekf_pose.header.frame_id; + pose_cov.pose.pose = current_ekf_pose.pose; pose_cov.pose.covariance = ekfCovarianceToPoseMessageCovariance(P); pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; - biased_pose_cov.pose.pose = current_biased_ekf_pose_.pose; + biased_pose_cov.pose.pose = current_biased_ekf_pose.pose; pub_biased_pose_cov_->publish(biased_pose_cov); /* publish latest twist */ - pub_twist_->publish(current_ekf_twist_); + pub_twist_->publish(current_ekf_twist); /* publish latest twist with covariance */ geometry_msgs::msg::TwistWithCovarianceStamped twist_cov; twist_cov.header.stamp = current_time; - twist_cov.header.frame_id = current_ekf_twist_.header.frame_id; - twist_cov.twist.twist = current_ekf_twist_.twist; + twist_cov.header.frame_id = current_ekf_twist.header.frame_id; + twist_cov.twist.twist = current_ekf_twist.twist; twist_cov.twist.covariance = ekfCovarianceToTwistMessageCovariance(P); pub_twist_cov_->publish(twist_cov); @@ -628,7 +645,7 @@ void EKFLocalizer::publishEstimateResult() /* publish latest odometry */ nav_msgs::msg::Odometry odometry; odometry.header.stamp = current_time; - odometry.header.frame_id = current_ekf_pose_.header.frame_id; + odometry.header.frame_id = current_ekf_pose.header.frame_id; odometry.child_frame_id = "base_link"; odometry.pose = pose_cov.pose; odometry.twist = twist_cov.twist; From 06d27aa743e1fb29087bbdb78422f8d13a64c17d Mon Sep 17 00:00:00 2001 From: "awf-autoware-bot[bot]" <94889083+awf-autoware-bot[bot]@users.noreply.github.com> Date: Fri, 27 Oct 2023 15:00:17 +0900 Subject: [PATCH 190/206] chore: update CODEOWNERS (#5350) Signed-off-by: GitHub Co-authored-by: github-actions --- .github/CODEOWNERS | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 9de27f82ffcef..e298dccefd827 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -88,13 +88,15 @@ launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp -localization/landmark_based_localizer/landmark_tf_caster/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_parser/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/localization_util/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/tree_structured_parzen_estimator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/twist2accel/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp @@ -152,7 +154,7 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@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 yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_crosswalk_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.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 @@ -179,7 +181,7 @@ planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.j planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp -planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp +planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @@ -221,7 +223,6 @@ system/emergency_handler/** makoto.kurihara@tier4.jp system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp system/system_diagnostic_graph/** isamu.takagi@tier4.jp -system/system_diagnostic_monitor/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp From 4e58c9a51fc9b69416e3dba73bbea1b86a2c4041 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Fri, 27 Oct 2023 15:13:14 +0900 Subject: [PATCH 191/206] refactor(ekf_localizer): use struct for diag info (#5421) * refactor(ekf_localizer): use struct for diag info Signed-off-by: kminoda * fix Signed-off-by: kminoda * style(pre-commit): autofix * update Signed-off-by: kminoda * style(pre-commit): autofix * move diag_info to ekf_localizer.hpp Signed-off-by: kminoda * remove diag.hpp Signed-off-by: kminoda --------- Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../include/ekf_localizer/ekf_localizer.hpp | 39 +++++++---- .../ekf_localizer/src/ekf_localizer.cpp | 70 ++++++++++--------- 2 files changed, 60 insertions(+), 49 deletions(-) diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 8da982b8c0dfa..719185da4306a 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -49,6 +49,28 @@ #include #include +struct EKFDiagnosticInfo +{ + EKFDiagnosticInfo() + : no_update_count(0), + queue_size(0), + is_passed_delay_gate(true), + delay_time(0), + delay_time_threshold(0), + is_passed_mahalanobis_gate(true), + mahalanobis_distance(0) + { + } + + size_t no_update_count; + size_t queue_size; + bool is_passed_delay_gate; + double delay_time; + double delay_time_threshold; + bool is_passed_mahalanobis_gate; + double mahalanobis_distance; +}; + class Simple1DFilter { public: @@ -167,21 +189,8 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; - size_t pose_no_update_count_; - size_t pose_queue_size_; - bool pose_is_passed_delay_gate_; - double pose_delay_time_; - double pose_delay_time_threshold_; - bool pose_is_passed_mahalanobis_gate_; - double pose_mahalanobis_distance_; - - size_t twist_no_update_count_; - size_t twist_queue_size_; - bool twist_is_passed_delay_gate_; - double twist_delay_time_; - double twist_delay_time_threshold_; - bool twist_is_passed_mahalanobis_gate_; - double twist_mahalanobis_distance_; + EKFDiagnosticInfo pose_diag_info_; + EKFDiagnosticInfo twist_diag_info_; AgedObjectQueue pose_queue_; AgedObjectQueue twist_queue_; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index b05ec80627570..c39e38c8d438d 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -176,12 +176,12 @@ void EKFLocalizer::timerCallback() /* pose measurement update */ - pose_queue_size_ = pose_queue_.size(); - pose_is_passed_delay_gate_ = true; - pose_delay_time_ = 0.0; - pose_delay_time_threshold_ = 0.0; - pose_is_passed_mahalanobis_gate_ = true; - pose_mahalanobis_distance_ = 0.0; + pose_diag_info_.queue_size = pose_queue_.size(); + pose_diag_info_.is_passed_delay_gate = true; + pose_diag_info_.delay_time = 0.0; + pose_diag_info_.delay_time_threshold = 0.0; + pose_diag_info_.is_passed_mahalanobis_gate = true; + pose_diag_info_.mahalanobis_distance = 0.0; bool pose_is_updated = false; @@ -201,16 +201,16 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } - pose_no_update_count_ = pose_is_updated ? 0 : (pose_no_update_count_ + 1); + pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1); /* twist measurement update */ - twist_queue_size_ = twist_queue_.size(); - twist_is_passed_delay_gate_ = true; - twist_delay_time_ = 0.0; - twist_delay_time_threshold_ = 0.0; - twist_is_passed_mahalanobis_gate_ = true; - twist_mahalanobis_distance_ = 0.0; + twist_diag_info_.queue_size = twist_queue_.size(); + twist_diag_info_.is_passed_delay_gate = true; + twist_diag_info_.delay_time = 0.0; + twist_diag_info_.delay_time_threshold = 0.0; + twist_diag_info_.is_passed_mahalanobis_gate = true; + twist_diag_info_.mahalanobis_distance = 0.0; bool twist_is_updated = false; @@ -230,7 +230,7 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } - twist_no_update_count_ = twist_is_updated ? 0 : (twist_no_update_count_ + 1); + twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); const geometry_msgs::msg::PoseStamped current_ekf_pose = getCurrentEKFPose(false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = getCurrentEKFPose(true); @@ -446,12 +446,12 @@ bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar delay_time = std::max(delay_time, 0.0); - int delay_step = std::roundf(delay_time / ekf_dt_); + const int delay_step = std::roundf(delay_time / ekf_dt_); - pose_delay_time_ = std::max(delay_time, pose_delay_time_); - pose_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; + pose_diag_info_.delay_time = std::max(delay_time, pose_diag_info_.delay_time); + pose_diag_info_.delay_time_threshold = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { - pose_is_passed_delay_gate_ = false; + pose_diag_info_.is_passed_delay_gate = false; warning_.warnThrottle( poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); return false; @@ -482,9 +482,9 @@ bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); - pose_mahalanobis_distance_ = std::max(distance, pose_mahalanobis_distance_); + pose_diag_info_.mahalanobis_distance = std::max(distance, pose_diag_info_.mahalanobis_distance); if (distance > params_.pose_gate_dist) { - pose_is_passed_mahalanobis_gate_ = false; + pose_diag_info_.is_passed_mahalanobis_gate = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); return false; @@ -542,12 +542,12 @@ bool EKFLocalizer::measurementUpdateTwist( } delay_time = std::max(delay_time, 0.0); - int delay_step = std::roundf(delay_time / ekf_dt_); + const int delay_step = std::roundf(delay_time / ekf_dt_); - twist_delay_time_ = std::max(delay_time, twist_delay_time_); - twist_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; + twist_diag_info_.delay_time = std::max(delay_time, twist_diag_info_.delay_time); + twist_diag_info_.delay_time_threshold = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { - twist_is_passed_delay_gate_ = false; + twist_diag_info_.is_passed_delay_gate = false; warning_.warnThrottle( twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); return false; @@ -571,9 +571,9 @@ bool EKFLocalizer::measurementUpdateTwist( const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); - twist_mahalanobis_distance_ = std::max(distance, twist_mahalanobis_distance_); + twist_diag_info_.mahalanobis_distance = std::max(distance, twist_diag_info_.mahalanobis_distance); if (distance > params_.twist_gate_dist) { - twist_is_passed_mahalanobis_gate_ = false; + twist_diag_info_.is_passed_mahalanobis_gate = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); return false; @@ -660,23 +660,25 @@ void EKFLocalizer::publishDiagnostics() if (is_activated_) { diag_status_array.push_back(checkMeasurementUpdated( - "pose", pose_no_update_count_, params_.pose_no_update_count_threshold_warn, + "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_queue_size_)); + diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_diag_info_.queue_size)); diag_status_array.push_back(checkMeasurementDelayGate( - "pose", pose_is_passed_delay_gate_, pose_delay_time_, pose_delay_time_threshold_)); + "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time, + pose_diag_info_.delay_time_threshold)); diag_status_array.push_back(checkMeasurementMahalanobisGate( - "pose", pose_is_passed_mahalanobis_gate_, pose_mahalanobis_distance_, + "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance, params_.pose_gate_dist)); diag_status_array.push_back(checkMeasurementUpdated( - "twist", twist_no_update_count_, params_.twist_no_update_count_threshold_warn, + "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn, params_.twist_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_queue_size_)); + diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_diag_info_.queue_size)); diag_status_array.push_back(checkMeasurementDelayGate( - "twist", twist_is_passed_delay_gate_, twist_delay_time_, twist_delay_time_threshold_)); + "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time, + twist_diag_info_.delay_time_threshold)); diag_status_array.push_back(checkMeasurementMahalanobisGate( - "twist", twist_is_passed_mahalanobis_gate_, twist_mahalanobis_distance_, + "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance, params_.twist_gate_dist)); } From 0af019ef4b3218122641e3cd883dffa7ebdf1a79 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 27 Oct 2023 16:38:20 +0900 Subject: [PATCH 192/206] fix(goal_planner): visualize stop wall (#5408) Signed-off-by: kosuke55 --- .../include/behavior_path_planner/utils/path_utils.hpp | 2 ++ .../scene_module/goal_planner/goal_planner_module.cpp | 2 ++ planning/behavior_path_planner/src/utils/path_utils.cpp | 9 +++++++++ 3 files changed, 13 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index b42502d9edec8..a9cbf859dd9d8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -104,6 +104,8 @@ PathWithLaneId calcCenterLinePath( const std::optional & prev_module_path = std::nullopt); PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); + +boost::optional getFirstStopPoseFromPath(const PathWithLaneId & path); } // namespace behavior_path_planner::utils #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 32b54a4e39170..d6f80697d9fd6 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -775,6 +775,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) } else { // not_safe -> not_safe: use previous stop path output.path = status_.get_prev_stop_path(); + stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path"); } @@ -804,6 +805,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output } else { // not_safe safe(no feasible stop path found) -> not_safe: use previous stop path output.path = status_.get_prev_stop_path_after_approval(); + stop_pose_ = utils::getFirstStopPoseFromPath(*output.path); RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, use previous stop path"); } output.reference_path = getPreviousModuleOutput().reference_path; diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 3ac3c09ba0d97..a6d7bc0040c23 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -580,4 +580,13 @@ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & return filtered_path; } +boost::optional getFirstStopPoseFromPath(const PathWithLaneId & path) +{ + for (const auto & p : path.points) { + if (std::abs(p.point.longitudinal_velocity_mps) < 0.01) { + return p.point.pose; + } + } + return boost::none; +} } // namespace behavior_path_planner::utils From b97ea6e64ed4217edebdd9a2a970b5e386ef5bf6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Fri, 27 Oct 2023 11:04:22 +0200 Subject: [PATCH 193/206] build(tracking_object_merger): remove no longer needed nlohmann_json dependency (#5429) Signed-off-by: Esteve Fernandez --- perception/tracking_object_merger/CMakeLists.txt | 2 -- .../data_association/data_association.hpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index 108749c5f91a7..ed5aa76afbfd9 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -7,7 +7,6 @@ endif() find_package(autoware_cmake REQUIRED) autoware_package() -find_package(nlohmann_json REQUIRED) # for debug # find dependencies @@ -36,7 +35,6 @@ ament_auto_add_library(decorative_tracker_merger_node SHARED target_link_libraries(decorative_tracker_merger_node mu_successive_shortest_path Eigen3::Eigen - nlohmann_json::nlohmann_json # for debug ) rclcpp_components_register_node(decorative_tracker_merger_node diff --git a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp index 05fc9f6caccb5..a62a39d11c192 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/data_association/data_association.hpp @@ -19,8 +19,6 @@ #ifndef TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ #define TRACKING_OBJECT_MERGER__DATA_ASSOCIATION__DATA_ASSOCIATION_HPP_ -// #include // for debug json library - #include #include #include From 29d34df028a4e0fa7f3482e0f7243be206dfc18f Mon Sep 17 00:00:00 2001 From: takeshi-iwanari Date: Fri, 27 Oct 2023 18:21:54 +0900 Subject: [PATCH 194/206] fix(system_monitor): output command line (#5430) * fix(system_monitor): output command line Signed-off-by: takeshi.iwanari * style(pre-commit): autofix --------- Signed-off-by: takeshi.iwanari Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../process_monitor/process_monitor.hpp | 2 +- .../src/process_monitor/process_monitor.cpp | 18 +++++++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index 42bd2a3ea9236..ec2a90c6b27e4 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -94,7 +94,7 @@ class ProcessMonitor : public rclcpp::Node * @param [out] command output command line * @return true if success to get command line name */ - bool getCommandLineFromPiD(const std::string & pid, std::string * command); + bool getCommandLineFromPiD(const std::string & pid, std::string & command); /** * @brief get top-rated processes diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index 6f58339f2ff4b..1173431b0f7c2 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -414,14 +414,22 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } -bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string * command) +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string & command) { std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; - std::ifstream commandFile(commandLineFilePath); + std::ifstream commandFile(commandLineFilePath, std::ios::in | std::ios::binary); + if (commandFile.is_open()) { - std::getline(commandFile, *command); + std::vector buffer; + std::copy( + std::istream_iterator(commandFile), std::istream_iterator(), + std::back_inserter(buffer)); commandFile.close(); - return true; + std::replace( + buffer.begin(), buffer.end(), '\0', + ' '); // 0x00 is used as delimiter in /cmdline instead of 0x20 (space) + command = std::string(buffer.begin(), buffer.end()); + return (buffer.size() > 0) ? true : false; // cmdline is empty if it is kernel process } else { return false; } @@ -478,7 +486,7 @@ void ProcessMonitor::getTopratedProcesses( std::string program_name; std::getline(stream, program_name); - bool flag_find_command_line = getCommandLineFromPiD(info.processId, &info.commandName); + bool flag_find_command_line = getCommandLineFromPiD(info.processId, info.commandName); if (!flag_find_command_line) { info.commandName = program_name; // if command line is not found, use program name instead From 25e8b3f5617de201db4f197be48615ff574b12f1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 27 Oct 2023 18:25:37 +0900 Subject: [PATCH 195/206] refactor(goal_planner): rebuild state transition (#5371)" (#5399)" (#5417) * refactor(goal_planner): rebuild state transition (#5371)" (#5399)" This reverts commit 92f78a476d47bf0811cbb69315fd403be574224f. * fix: increment path idx Signed-off-by: kosuke55 * refactor Signed-off-by: kosuke55 --------- Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.hpp | 31 +++++----- .../goal_planner/goal_planner_module.cpp | 61 ++++++------------- 2 files changed, 36 insertions(+), 56 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index c5b2209c16f19..df21bad862cc9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -135,10 +135,8 @@ class PullOverStatus DEFINE_SETTER_GETTER(bool, has_decided_velocity) DEFINE_SETTER_GETTER(bool, has_requested_approval) DEFINE_SETTER_GETTER(bool, is_ready) - DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_time) DEFINE_SETTER_GETTER(std::shared_ptr, last_increment_time) DEFINE_SETTER_GETTER(std::shared_ptr, last_path_update_time) - DEFINE_SETTER_GETTER(std::shared_ptr, last_approved_pose) DEFINE_SETTER_GETTER(std::optional, modified_goal_pose) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) DEFINE_SETTER_GETTER(GoalCandidates, goal_candidates) @@ -166,10 +164,8 @@ class PullOverStatus bool is_ready_{false}; // save last time and pose - std::shared_ptr last_approved_time_; std::shared_ptr last_increment_time_; std::shared_ptr last_path_update_time_; - std::shared_ptr last_approved_pose_; // goal modification std::optional modified_goal_pose_; @@ -199,6 +195,14 @@ struct GoalPlannerDebugData std::vector ego_polygons_expanded{}; }; +struct LastApprovalData +{ + LastApprovalData(rclcpp::Time time, Pose pose) : time(time), pose(pose) {} + + rclcpp::Time time{}; + Pose pose{}; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -219,14 +223,11 @@ class GoalPlannerModule : public SceneModuleInterface } } - // TODO(someone): remove this, and use base class function - [[deprecated]] BehaviorModuleOutput run() override; - bool isExecutionRequested() const override; - bool isExecutionReady() const override; - // TODO(someone): remove this, and use base class function - [[deprecated]] ModuleStatus updateState() override; + CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; + bool isExecutionRequested() const override; + bool isExecutionReady() const override; void processOnExit() override; void updateData() override; void setParameters(const std::shared_ptr & parameters); @@ -235,15 +236,15 @@ class GoalPlannerModule : public SceneModuleInterface { } - // not used, but need to override - CandidateOutput planCandidate() const override { return CandidateOutput{}; }; - private: + // The start_planner activates when it receives a new route, + // so there is no need to terminate the goal planner. + // If terminating it, it may switch to lane following and could generate an inappropriate path. bool canTransitSuccessState() override { return false; } bool canTransitFailureState() override { return false; } - bool canTransitIdleToRunningState() override { return false; } + bool canTransitIdleToRunningState() override { return true; } mutable StartGoalPlannerData goal_planner_data_; @@ -276,6 +277,8 @@ class GoalPlannerModule : public SceneModuleInterface std::recursive_mutex mutex_; PullOverStatus status_; + std::unique_ptr last_approval_data_{nullptr}; + // approximate distance from the start point to the end point of pull_over. // this is used as an assumed value to decelerate, etc., before generating the actual path. const double approximate_pull_over_distance_{20.0}; diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index d6f80697d9fd6..8b6ac5394c3d4 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -227,18 +227,6 @@ void GoalPlannerModule::onFreespaceParkingTimer() } } -BehaviorModuleOutput GoalPlannerModule::run() -{ - current_state_ = ModuleStatus::RUNNING; - updateOccupancyGrid(); - - if (!isActivated()) { - return planWaitingApproval(); - } - - return plan(); -} - void GoalPlannerModule::updateData() { // Initialize Occupancy Grid Map @@ -287,6 +275,8 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); + status_.reset(); + last_approval_data_.reset(); } bool GoalPlannerModule::isExecutionRequested() const @@ -448,13 +438,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const return refined_goal_pose; } -ModuleStatus GoalPlannerModule::updateState() -{ - // start_planner module will be run when setting new goal, so not need finishing pull_over module. - // Finishing it causes wrong lane_following path generation. - return current_state_; -} - bool GoalPlannerModule::planFreespacePath() { goal_searcher_->setPlannerData(planner_data_); @@ -914,6 +897,12 @@ void GoalPlannerModule::decideVelocity() status_.set_has_decided_velocity(true); } +CandidateOutput GoalPlannerModule::planCandidate() const +{ + return CandidateOutput( + status_.get_pull_over_path() ? status_.get_pull_over_path()->getFullPath() : PathWithLaneId()); +} + BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() { // if pull over path candidates generation is not finished, use previous module output @@ -931,14 +920,9 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // Use decided path if (status_.get_has_decided_path()) { - if (isActivated() && isWaitingApproval()) { - { - const std::lock_guard lock(mutex_); - status_.set_last_approved_time(std::make_shared(clock_->now())); - status_.set_last_approved_pose( - std::make_shared(planner_data_->self_odometry->pose.pose)); - } - clearWaitingApproval(); + if (isActivated() && !last_approval_data_) { + last_approval_data_ = + std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); decideVelocity(); } transitionToNextPathIfFinishingCurrentPath(); @@ -954,7 +938,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() // set output and status BehaviorModuleOutput output{}; setOutput(output); - path_candidate_ = std::make_shared(status_.get_pull_over_path()->getFullPath()); + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible @@ -1005,16 +989,11 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( return getPreviousModuleOutput(); } - waitApproval(); - BehaviorModuleOutput out; out.modified_goal = plan().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; - path_candidate_ = - status_.get_is_safe_static_objects() - ? std::make_shared(status_.get_pull_over_path()->getFullPath()) - : out.path; + path_candidate_ = std::make_shared(planCandidate().path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; const auto distance_to_path_change = calcDistanceToPathChange(); @@ -1191,12 +1170,11 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { - if (isActivated() && status_.get_last_approved_time()) { + if (isActivated() && last_approval_data_) { // if using arc_path and finishing current_path, get next path // enough time for turn signal - const bool has_passed_enough_time = - (clock_->now() - *status_.get_last_approved_time()).seconds() > - planner_data_->parameters.turn_signal_search_time; + const bool has_passed_enough_time = (clock_->now() - last_approval_data_->time).seconds() > + planner_data_->parameters.turn_signal_search_time; if (hasFinishedCurrentPath() && has_passed_enough_time && status_.get_require_increment()) { if (incrementPathIndex()) { @@ -1331,10 +1309,9 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { // ego decelerates so that current pose is the point `turn_light_on_threshold_time` seconds // before starting pull_over - turn_signal.desired_start_point = - status_.get_last_approved_pose() && status_.get_has_decided_path() - ? *status_.get_last_approved_pose() - : current_pose; + turn_signal.desired_start_point = last_approval_data_ && status_.get_has_decided_path() + ? last_approval_data_->pose + : current_pose; turn_signal.desired_end_point = end_pose; turn_signal.required_start_point = start_pose; turn_signal.required_end_point = end_pose; From 124dcf41a83ece1c6f16b700867bd697040230f5 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Sat, 28 Oct 2023 16:00:55 +0900 Subject: [PATCH 196/206] fix(goal_planner): do not generate path candidates before execution (#5433) Signed-off-by: kosuke55 --- .../goal_planner/goal_planner_module.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 8b6ac5394c3d4..3c470cef2c935 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -146,6 +146,17 @@ void GoalPlannerModule::onTimer() if (status_.get_goal_candidates().empty()) { return; } + + if (!isExecutionRequested()) { + return; + } + + if ( + !planner_data_ || + !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler)) { + return; + } + const auto goal_candidates = status_.get_goal_candidates(); // generate valid pull over path candidates and calculate closest start pose @@ -229,6 +240,10 @@ void GoalPlannerModule::onFreespaceParkingTimer() void GoalPlannerModule::updateData() { + if (getCurrentStatus() == ModuleStatus::IDLE) { + return; + } + // Initialize Occupancy Grid Map // This operation requires waiting for `planner_data_`, hence it is executed here instead of in // the constructor. Ideally, this operation should only need to be performed once. @@ -990,7 +1005,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( } BehaviorModuleOutput out; - out.modified_goal = plan().modified_goal; // update status_ + out.modified_goal = planWithGoalModification().modified_goal; // update status_ out.path = std::make_shared(generateStopPath()); out.reference_path = getPreviousModuleOutput().reference_path; path_candidate_ = std::make_shared(planCandidate().path_candidate); From 02b2be9081978c04544ee3cc0f308e5d2f459af0 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 30 Oct 2023 01:37:47 +0900 Subject: [PATCH 197/206] fix(lane_change): separate backward buffer for blocking object (#5434) * fix(lane_change): separate backward buffer for blocking object Signed-off-by: Fumiya Watanabe * Update planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp Co-authored-by: Kosuke Takeuchi * style(pre-commit): autofix --------- Signed-off-by: Fumiya Watanabe Co-authored-by: Kosuke Takeuchi Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../config/lane_change/lane_change.param.yaml | 1 + .../behavior_path_planner/parameters.hpp | 1 + .../behavior_path_planner/utils/utils.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 2 + .../scene_module/lane_change/interface.cpp | 4 +- .../src/scene_module/lane_change/normal.cpp | 39 ++++++++++++------- .../src/utils/lane_change/utils.cpp | 4 +- .../behavior_path_planner/src/utils/utils.cpp | 5 +-- 8 files changed, 35 insertions(+), 23 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index d3c0a22e867f9..f98eac5315006 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -5,6 +5,7 @@ prepare_duration: 4.0 # [s] backward_length_buffer_for_end_of_lane: 3.0 # [m] + backward_length_buffer_for_blocking_object: 3.0 # [m] lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index a4e6fc18ab050..5dceeeff3aada 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -92,6 +92,7 @@ struct BehaviorPathPlannerParameters double backward_path_length; double forward_path_length; double backward_length_buffer_for_end_of_lane; + double backward_length_buffer_for_blocking_object; double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 326a6aded5e88..38c2620096ca2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -392,7 +392,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre double calcMinimumLaneChangeLength( const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double length_to_intersection = 0.0); + const double backward_buffer, const double length_to_intersection = 0.0); lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 0777822650dfb..bd46bf069e846 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -340,6 +340,8 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() // lane change parameters p.backward_length_buffer_for_end_of_lane = declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); + p.backward_length_buffer_for_blocking_object = + declare_parameter("lane_change.backward_length_buffer_for_blocking_object"); p.lane_changing_lateral_jerk = declare_parameter("lane_change.lane_changing_lateral_jerk"); p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 22c8ac022ad1a..be3b78803c4c6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -453,8 +453,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & common_parameter = module_type_->getCommonParam(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double next_lane_change_buffer = - utils::calcMinimumLaneChangeLength(common_parameter, shift_intervals); + const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( + common_parameter, shift_intervals, common_parameter.backward_length_buffer_for_end_of_lane); const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; const double & base_to_front = common_parameter.base_link2front; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d887b570b753a..58a8e7e181e35 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -200,8 +200,9 @@ void NormalLaneChange::insertStopPoint( } const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, + 0.0); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -263,10 +264,14 @@ void NormalLaneChange::insertStopPoint( const auto rss_dist = calcRssDistance( 0.0, planner_data_->parameters.minimum_lane_changing_velocity, lane_change_parameters_->rss_params); + const double lane_change_buffer_for_blocking_object = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, + getCommonParam().backward_length_buffer_for_blocking_object, 0.0); - const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer - - stop_point_buffer - rss_dist - - getCommonParam().base_link2front; + const auto stopping_distance_for_obj = + distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - + getCommonParam().backward_length_buffer_for_blocking_object - rss_dist - + getCommonParam().base_link2front; // If the target lane in the lane change section is blocked by a stationary obstacle, there // is no reason for stopping with a lane change margin. Instead, stop right behind the @@ -461,8 +466,9 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & current_pose = getEgoPose(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const auto lane_change_buffer = - utils::calcMinimumLaneChangeLength(planner_data_->parameters, shift_intervals); + const auto lane_change_buffer = utils::calcMinimumLaneChangeLength( + planner_data_->parameters, shift_intervals, + getCommonParam().backward_length_buffer_for_end_of_lane); auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -989,8 +995,8 @@ bool NormalLaneChange::hasEnoughLength( const double lane_change_length = path.info.length.sum(); const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); - double minimum_lane_change_length_to_preferred_lane = - utils::calcMinimumLaneChangeLength(common_parameters, shift_intervals); + double minimum_lane_change_length_to_preferred_lane = utils::calcMinimumLaneChangeLength( + common_parameters, shift_intervals, common_parameters.backward_length_buffer_for_end_of_lane); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1086,9 +1092,11 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), + common_parameters.backward_length_buffer_for_end_of_lane); const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()), + common_parameters.backward_length_buffer_for_end_of_lane); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); @@ -1489,8 +1497,8 @@ bool NormalLaneChange::getAbortPath() const auto direction = getDirection(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( selected_path.info.current_lanes.back(), direction); - const double minimum_lane_change_length = - utils::calcMinimumLaneChangeLength(common_param, shift_intervals); + const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( + common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1743,8 +1751,9 @@ bool NormalLaneChange::isVehicleStuck( : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = - utils::calcMinimumLaneChangeLength(getCommonParam(), shift_intervals, 0.0); + const double lane_change_buffer = utils::calcMinimumLaneChangeLength( + getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, + 0.0); const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index d5867c40824d2..fb2a96f1d5cbd 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -604,8 +604,8 @@ bool hasEnoughLengthToLaneChangeAfterAbort( { const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); - const double minimum_lane_change_length = - utils::calcMinimumLaneChangeLength(common_param, shift_intervals); + const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( + common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 9737db21a31a5..3ab20e7f76718 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -3310,7 +3310,7 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre double calcMinimumLaneChangeLength( const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double length_to_intersection) + const double backward_buffer, const double length_to_intersection) { if (shift_intervals.empty()) { return 0.0; @@ -3328,8 +3328,7 @@ double calcMinimumLaneChangeLength( PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); accumulated_length += vel * t + finish_judge_buffer; } - accumulated_length += - common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); + accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); return accumulated_length; } From 0c5abbd7d30d3bc6df7eabcd3d8b57a5739ae86a Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Mon, 30 Oct 2023 08:47:30 +0900 Subject: [PATCH 198/206] refactor(ar_tag_based_localizer): add test (#5425) * Added test to ar_tag_based_localizer Signed-off-by: Shintaro Sakoda * Fixed to follow clang-tidy Signed-off-by: Shintaro Sakoda * Renamed files Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Added include Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../ar_tag_based_localizer/CMakeLists.txt | 17 ++++- ...er_core.cpp => ar_tag_based_localizer.cpp} | 9 +-- .../ar_tag_based_localizer.hpp} | 8 +-- ..._tag_based_localizer_node.cpp => main.cpp} | 2 +- .../ar_tag_based_localizer/test/test.cpp | 67 +++++++++++++++++++ 5 files changed, 92 insertions(+), 11 deletions(-) rename localization/landmark_based_localizer/ar_tag_based_localizer/src/{ar_tag_based_localizer_core.cpp => ar_tag_based_localizer.cpp} (98%) rename localization/landmark_based_localizer/ar_tag_based_localizer/{include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp => src/ar_tag_based_localizer.hpp} (95%) rename localization/landmark_based_localizer/ar_tag_based_localizer/src/{ar_tag_based_localizer_node.cpp => main.cpp} (97%) create mode 100644 localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index 15aecd1f8ded9..d625064b8f6cb 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -15,8 +15,8 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) ament_auto_add_executable(ar_tag_based_localizer - src/ar_tag_based_localizer_node.cpp - src/ar_tag_based_localizer_core.cpp + src/main.cpp + src/ar_tag_based_localizer.cpp ) target_include_directories(ar_tag_based_localizer SYSTEM PUBLIC @@ -24,6 +24,19 @@ target_include_directories(ar_tag_based_localizer ) target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_ar_tag_based_localizer + test/test.cpp + src/ar_tag_based_localizer.cpp + ) + target_include_directories(test_ar_tag_based_localizer + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} + ) + target_link_libraries(test_ar_tag_based_localizer ${OpenCV_LIBRARIES}) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp similarity index 98% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 0492e07875a17..73e7b8b7e3587 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -42,7 +42,7 @@ or implied, of Rafael Muñoz Salinas. ********************************/ -#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" +#include "ar_tag_based_localizer.hpp" #include #include @@ -51,14 +51,15 @@ #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif -ArTagBasedLocalizer::ArTagBasedLocalizer() -: Node("ar_tag_based_localizer"), cam_info_received_(false) +ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) +: Node("ar_tag_based_localizer", options), cam_info_received_(false) { } @@ -219,7 +220,7 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha image_pub_.publish(out_msg.toImageMsg()); } - const int detected_tags = markers.size(); + const int detected_tags = static_cast(markers.size()); diagnostic_msgs::msg::DiagnosticStatus diag_status; diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp similarity index 95% rename from localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index facd02e2b9b7b..6bd66eead653f 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -42,8 +42,8 @@ or implied, of Rafael Muñoz Salinas. ********************************/ -#ifndef AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ -#define AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ +#ifndef AR_TAG_BASED_LOCALIZER_HPP_ +#define AR_TAG_BASED_LOCALIZER_HPP_ #include "landmark_parser/landmark_parser_core.hpp" @@ -67,7 +67,7 @@ class ArTagBasedLocalizer : public rclcpp::Node { public: - ArTagBasedLocalizer(); + explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); bool setup(); private: @@ -115,4 +115,4 @@ class ArTagBasedLocalizer : public rclcpp::Node std::map landmark_map_; }; -#endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ +#endif // AR_TAG_BASED_LOCALIZER_HPP_ diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp similarity index 97% rename from localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp index 92d2e35ee3c1b..cbcd57b4b222a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/main.cpp @@ -42,7 +42,7 @@ or implied, of Rafael Muñoz Salinas. ********************************/ -#include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" +#include "ar_tag_based_localizer.hpp" int main(int argc, char ** argv) { diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp new file mode 100644 index 0000000000000..6b302ee6bc862 --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/test/test.cpp @@ -0,0 +1,67 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "../src/ar_tag_based_localizer.hpp" + +#include + +#include +#include + +#include +#include +#include + +class TestArTagBasedLocalizer : public ::testing::Test +{ +protected: + void SetUp() override + { + const std::string yaml_path = + "../../install/ar_tag_based_localizer/share/ar_tag_based_localizer/config/" + "ar_tag_based_localizer.param.yaml"; + + rcl_params_t * params_st = rcl_yaml_node_struct_init(rcl_get_default_allocator()); + if (!rcl_parse_yaml_file(yaml_path.c_str(), params_st)) { + std::cout << "Failed to parse yaml file" << std::endl; + return; + } + + const rclcpp::ParameterMap param_map = rclcpp::parameter_map_from(params_st, ""); + rclcpp::NodeOptions node_options; + for (const auto & param_pair : param_map) { + for (const auto & param : param_pair.second) { + node_options.parameter_overrides().push_back(param); + } + } + node_ = std::make_shared(node_options); + rcl_yaml_node_struct_fini(params_st); + } + + std::shared_ptr node_; +}; + +TEST_F(TestArTagBasedLocalizer, test_setup) // NOLINT +{ + EXPECT_TRUE(node_->setup()); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} From 65f0a71b75eb4ea438153e4e154c296502f058ab Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 30 Oct 2023 10:29:27 +0900 Subject: [PATCH 199/206] feat(behavior_path_planner): subscribe traffic light recognition result (#5436) feat(avoidance): use traffic light signal info Signed-off-by: satoshi-ota --- .../behavior_planning.launch.py | 4 +++ .../behavior_planning.launch.xml | 1 + .../config/behavior_path_planner.param.yaml | 1 + .../behavior_path_planner_node.hpp | 3 +++ .../behavior_path_planner/data_manager.hpp | 27 +++++++++++++++++++ .../behavior_path_planner/parameters.hpp | 1 + planning/behavior_path_planner/package.xml | 1 + .../src/behavior_path_planner_node.cpp | 16 +++++++++++ 8 files changed, 54 insertions(+) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 62d4c5b7188ee..f535fe1184d74 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -84,6 +84,10 @@ def launch_setup(context, *args, **kwargs): "~/input/costmap", "/planning/scenario_planning/parking/costmap_generator/occupancy_grid", ), + ( + "~/input/traffic_signals", + "/perception/traffic_light_recognition/traffic_signals", + ), ("~/input/odometry", "/localization/kinematic_state"), ("~/input/accel", "/localization/acceleration"), ("~/input/scenario", "/planning/scenario_planning/scenario"), diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 4ff862c7852c6..8ffa682030a99 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -8,6 +8,7 @@ + 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 d333cbfd778cb..b4d323de45cde 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -2,6 +2,7 @@ ros__parameters: verbose: false max_iteration_num: 100 + traffic_light_signal_timeout: 1.0 planning_hz: 10.0 backward_path_length: 5.0 forward_path_length: 300.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 420a5a8aa6ee5..1a9b23be00a3f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -58,6 +58,7 @@ using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::PoseWithUuidStamped; using nav_msgs::msg::OccupancyGrid; @@ -93,6 +94,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; + rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; rclcpp::Subscription::SharedPtr operation_mode_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; @@ -136,6 +138,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onCostMap(const OccupancyGrid::ConstSharedPtr msg); + void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg); void onMap(const HADMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 9280a81e643ca..ca58144464731 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -39,6 +41,7 @@ #include #include +#include #include #include #include @@ -51,6 +54,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_perception_msgs::msg::TrafficSignal; using autoware_planning_msgs::msg::PoseWithUuidStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; @@ -59,8 +63,15 @@ using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; using PlanResult = PathWithLaneId::SharedPtr; +using lanelet::TrafficLight; using unique_identifier_msgs::msg::UUID; +struct TrafficSignalStamped +{ + builtin_interfaces::msg::Time stamp; + TrafficSignal signal; +}; + struct BoolStamped { explicit BoolStamped(bool in_data) : data(in_data) {} @@ -145,6 +156,7 @@ struct PlannerData std::optional prev_modified_goal{}; std::optional prev_route_id{}; std::shared_ptr route_handler{std::make_shared()}; + std::map traffic_light_id_map; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; @@ -162,6 +174,21 @@ struct PlannerData route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data); } + std::optional getTrafficSignal(const int id) const + { + if (traffic_light_id_map.count(id) == 0) { + return std::nullopt; + } + + const auto elapsed_time = + (rclcpp::Clock{RCL_ROS_TIME}.now() - traffic_light_id_map.at(id).stamp).seconds(); + if (elapsed_time > parameters.traffic_light_signal_timeout) { + return std::nullopt; + } + + return traffic_light_id_map.at(id); + } + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 5dceeeff3aada..1998e7b99dd65 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -77,6 +77,7 @@ struct BehaviorPathPlannerParameters { bool verbose; size_t max_iteration_num{100}; + double traffic_light_signal_timeout{1.0}; ModuleConfigParameters config_avoidance; ModuleConfigParameters config_avoidance_by_lc; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 24ee1056e5a47..95ad5f1efb9ac 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -46,6 +46,7 @@ autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs + autoware_perception_msgs behaviortree_cpp_v3 freespace_planning_algorithms geometry_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index bd46bf069e846..ef44bee663985 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -106,6 +106,10 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod costmap_subscriber_ = create_subscription( "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), createSubscriptionOptions(this)); + traffic_signals_subscriber_ = + this->create_subscription( + "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1), + createSubscriptionOptions(this)); lateral_offset_subscriber_ = this->create_subscription( "~/input/lateral_offset", 1, std::bind(&BehaviorPathPlannerNode::onLateralOffset, this, _1), createSubscriptionOptions(this)); @@ -277,6 +281,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.verbose = declare_parameter("verbose"); p.max_iteration_num = declare_parameter("max_iteration_num"); + p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); const auto get_scene_module_manager_param = [&](std::string && ns) { ModuleConfigParameters config; @@ -934,6 +939,17 @@ void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) const std::lock_guard lock(mutex_pd_); planner_data_->costmap = msg; } +void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg) +{ + std::lock_guard lock(mutex_pd_); + + for (const auto & signal : msg->signals) { + TrafficSignalStamped traffic_signal; + traffic_signal.stamp = msg->stamp; + traffic_signal.signal = signal; + planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + } +} void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_map_); From 6632d79aa04841de805b442999882906b3f659b6 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 30 Oct 2023 12:18:02 +0900 Subject: [PATCH 200/206] feat(radar_object_tracker): tune radar object tracker node for untrustable radar input (#5409) * fix include file Signed-off-by: yoshiri * switchable trust/untrust input yaw and twist information via parameters Signed-off-by: yoshiri * Set measurement count threshold tunable Signed-off-by: yoshiri * enable to switch off acceleration estimation Signed-off-by: yoshiri * add constant turn rate tracker model Signed-off-by: yoshiri * tune default parameters Signed-off-by: yoshiri * set orientation unreliable when object is yaw input is not trusted Signed-off-by: yoshiri * set ekf parameter to static so that it reduces yaml loading function calls Signed-off-by: yoshiri * fix launch file to load tracking_config_directory as arguments Signed-off-by: yoshiri * update readme Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../radar_object_tracker/CMakeLists.txt | 1 + perception/radar_object_tracker/README.md | 11 +- .../config/data_association_matrix.param.yaml | 8 +- .../config/default_tracker.param.yaml | 14 +- .../config/radar_object_tracker.param.yaml | 9 +- .../constant_turn_rate_motion_tracker.yaml | 36 + .../tracking/linear_motion_tracker.yaml | 24 +- .../radar_object_tracker_node.hpp | 2 + .../constant_turn_rate_motion_tracker.hpp | 112 ++++ .../tracker/model/linear_motion_tracker.hpp | 23 +- .../radar_object_tracker/tracker/tracker.hpp | 1 + .../launch/radar_object_tracker.launch.xml | 4 +- .../radar_object_tracker_node.cpp | 8 +- .../constant_turn_rate_motion_tracker.cpp | 624 ++++++++++++++++++ .../tracker/model/linear_motion_tracker.cpp | 125 +++- 15 files changed, 931 insertions(+), 71 deletions(-) create mode 100644 perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml create mode 100644 perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp create mode 100644 perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 424e2b2d2c13e..3e3afacadd00f 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -27,6 +27,7 @@ ament_auto_add_library(radar_object_tracker_node SHARED src/radar_object_tracker_node/radar_object_tracker_node.cpp src/tracker/model/tracker_base.cpp src/tracker/model/linear_motion_tracker.cpp + src/tracker/model/constant_turn_rate_motion_tracker.cpp src/utils/utils.cpp src/data_association/data_association.cpp ) diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index 739a4a745ff6c..395bc025c2821 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -46,7 +46,7 @@ See more details in the [models.md](models.md). | `publish_rate` | double | 10.0 | The rate at which to publish the output messages | | `world_frame_id` | string | "map" | The frame ID of the world coordinate system | | `enable_delay_compensation` | bool | false | Whether to enable delay compensation. If set to `true`, output topic is published by timer with `publish_rate`. | -| `tracking_config_directory` | string | "" | The directory containing the tracking configuration files | +| `tracking_config_directory` | string | "./config/tracking/" | The directory containing the tracking configuration files | | `enable_logging` | bool | false | Whether to enable logging | | `logging_file_path` | string | "/tmp/association_log.json" | The path to the file where logs should be written | | `tracker_lifetime` | double | 1.0 | The lifetime of the tracker in seconds | @@ -65,6 +65,15 @@ See more details in the [models.md](models.md). See more details in the [models.md](models.md). +### Tracker parameters + +Currently, this package supports the following trackers: + +- `linear_motion_tracker` +- `constant_turn_rate_motion_tracker` + +Default settings for each tracker are defined in the [./config/tracking/](./config/tracking/), and described in [models.md](models.md). + ## Assumptions / Known limits diff --git a/perception/radar_object_tracker/config/data_association_matrix.param.yaml b/perception/radar_object_tracker/config/data_association_matrix.param.yaml index 104d790d185db..69628414e67d4 100644 --- a/perception/radar_object_tracker/config/data_association_matrix.param.yaml +++ b/perception/radar_object_tracker/config/data_association_matrix.param.yaml @@ -14,10 +14,10 @@ max_dist_matrix: #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN - 4.0, 4.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER + 4.0, 8.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #CAR + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRUCK + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #BUS + 4.0, 6.0, 8.0, 8.0, 8.0, 1.0, 1.0, 1.0, #TRAILER 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN diff --git a/perception/radar_object_tracker/config/default_tracker.param.yaml b/perception/radar_object_tracker/config/default_tracker.param.yaml index 757125202d69b..6c26034860e1b 100644 --- a/perception/radar_object_tracker/config/default_tracker.param.yaml +++ b/perception/radar_object_tracker/config/default_tracker.param.yaml @@ -1,9 +1,9 @@ /**: ros__parameters: - car_tracker: "linear_motion_tracker" - truck_tracker: "linear_motion_tracker" - bus_tracker: "linear_motion_tracker" - trailer_tracker: "linear_motion_tracker" - pedestrian_tracker: "linear_motion_tracker" - bicycle_tracker: "linear_motion_tracker" - motorcycle_tracker: "linear_motion_tracker" + car_tracker: "constant_turn_rate_motion_tracker" + truck_tracker: "constant_turn_rate_motion_tracker" + bus_tracker: "constant_turn_rate_motion_tracker" + trailer_tracker: "constant_turn_rate_motion_tracker" + pedestrian_tracker: "constant_turn_rate_motion_tracker" + bicycle_tracker: "constant_turn_rate_motion_tracker" + motorcycle_tracker: "constant_turn_rate_motion_tracker" diff --git a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml index f80adffb41090..d2c0841cf372e 100644 --- a/perception/radar_object_tracker/config/radar_object_tracker.param.yaml +++ b/perception/radar_object_tracker/config/radar_object_tracker.param.yaml @@ -4,12 +4,11 @@ # basic settings world_frame_id: "map" tracker_lifetime: 1.0 # [sec] - # if empty, use default config declared in this package - tracking_config_directory: "" + measurement_count_threshold: 3 # object will be published if it is tracked more than this threshold # delay compensate parameters publish_rate: 10.0 - enable_delay_compensation: false + enable_delay_compensation: true # logging enable_logging: false @@ -18,10 +17,10 @@ # filtering ## 1. distance based filtering: remove closer objects than this threshold use_distance_based_noise_filtering: true - minimum_range_threshold: 70.0 # [m] + minimum_range_threshold: 60.0 # [m] ## 2. lanelet map based filtering use_map_based_noise_filtering: true max_distance_from_lane: 5.0 # [m] max_angle_diff_from_lane: 0.785398 # [rad] (45 deg) - max_lateral_velocity: 5.0 # [m/s] + max_lateral_velocity: 7.0 # [m/s] diff --git a/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml b/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml new file mode 100644 index 0000000000000..f80f881cabf03 --- /dev/null +++ b/perception/radar_object_tracker/config/tracking/constant_turn_rate_motion_tracker.yaml @@ -0,0 +1,36 @@ +default: + # This file defines the parameters for the linear motion tracker. + # All this parameter coordinate is assumed to be in the vehicle coordinate system. + # So, the x axis is pointing to the front of the vehicle, y axis is pointing to the left of the vehicle. + ekf_params: + # random walk noise is used to model the acceleration noise + process_noise_std: # [m/s^2] + x: 0.5 + y: 0.5 + yaw: 0.1 + vx: 1.0 # assume 1m/s velocity noise + wz: 0.4 + measurement_noise_std: + x: 4.0 # [m] + y: 4.0 # [m] + # y: 0.02 # rad/m if use_polar_coordinate_in_measurement_noise is true + yaw: 0.2 # [rad] + vx: 10 # [m/s] + initial_covariance_std: + x: 3.0 # [m] + y: 6.0 # [m] + yaw: 10.0 # [rad] + vx: 100.0 # [m/s] + wz: 10.0 # [rad/s] + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: false # set true if you want to define the measurement noise in polar coordinate + assume_zero_yaw_rate: false # set true if you want to assume zero yaw rate + # output limitation + limit: + max_speed: 80.0 # [m/s] + # low pass filter is used to smooth the yaw and shape estimation + low_pass_filter: + time_constant: 1.0 # [s] + sampling_time: 0.1 # [s] diff --git a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml index 71367f4575193..5e813558a2bff 100644 --- a/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml +++ b/perception/radar_object_tracker/config/tracking/linear_motion_tracker.yaml @@ -7,22 +7,28 @@ default: process_noise_std: # [m/s^2] ax: 0.98 # assume 0.1G acceleration noise ay: 0.98 - vx: 0.1 # assume 0.1m/s velocity noise - vy: 0.1 + vx: 1.0 # assume 1m/s velocity noise + vy: 1.0 x: 1.0 # assume 1m position noise y: 1.0 measurement_noise_std: x: 0.6 # [m] - y: 0.9 # [m] - vx: 0.4 # [m/s] - vy: 1 # [m/s] + # y: 4.0 # [m] + y: 0.01 # rad/m if use_polar_coordinate_in_measurement_noise is true + vx: 10 # [m/s] + vy: 100 # [m/s] initial_covariance_std: x: 3.0 # [m] y: 6.0 # [m] - vx: 1.0 # [m/s] - vy: 5.0 # [m/s] - ax: 0.5 # [m/s^2] - ay: 1.0 # [m/s^2] + vx: 1000.0 # [m/s] + vy: 1000.0 # [m/s] + ax: 1000.0 # [m/s^2] + ay: 1000.0 # [m/s^2] + estimate_acc: false # set true if you want to estimate the acceleration + # input flag + trust_yaw_input: false # set true if yaw input of sensor is reliable + trust_twist_input: false # set true if twist input of sensor is reliable + use_polar_coordinate_in_measurement_noise: true # set true if you want to define the measurement noise in polar coordinate # output limitation limit: max_speed: 80.0 # [m/s] diff --git a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp index 415ff24b34cc3..9a3fdf602a3ca 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp @@ -76,6 +76,8 @@ class RadarObjectTrackerNode : public rclcpp::Node float tracker_lifetime_; std::map tracker_map_; + int measurement_count_threshold_; + void onMeasurement( const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_objects_msg); void onTimer(); diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp new file mode 100644 index 0000000000000..ac4b2cd1a0acb --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp @@ -0,0 +1,112 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ +#define RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ + +#include "radar_object_tracker/tracker/model/tracker_base.hpp" + +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +class ConstantTurnRateMotionTracker : public Tracker // means constant turn rate motion tracker +{ +private: + autoware_auto_perception_msgs::msg::DetectedObject object_; + rclcpp::Logger logger_; + +private: + KalmanFilter ekf_; + rclcpp::Time last_update_time_; + enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, WZ = 4 }; + + struct EkfParams + { + // dimension + char dim_x = 5; + // system noise + double q_cov_x; + double q_cov_y; + double q_cov_yaw; + double q_cov_vx; + double q_cov_wz; + // measurement noise + double r_cov_x; + double r_cov_y; + double r_cov_yaw; + double r_cov_vx; + // initial state covariance + double p0_cov_x; + double p0_cov_y; + double p0_cov_yaw; + double p0_cov_vx; + double p0_cov_wz; + }; + static EkfParams ekf_params_; + + // limitation + static double max_vx_; + // rough tracking parameters + float z_; + + // lpf parameter + static double filter_tau_; // time constant of 1st order low pass filter + static double filter_dt_; // sampling time of 1st order low pass filter + + // static flags + static bool is_initialized_; + static bool trust_yaw_input_; + static bool trust_twist_input_; + static bool use_polar_coordinate_in_measurement_noise_; + static bool assume_zero_yaw_rate_; + +private: + struct BoundingBox + { + double length; + double width; + double height; + }; + struct Cylinder + { + double width; + double height; + }; + BoundingBox bounding_box_; + Cylinder cylinder_; + +public: + ConstantTurnRateMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & tracker_param_file, const std::uint8_t & label); + + static void loadDefaultModelParameters(const std::string & path); + bool predict(const rclcpp::Time & time) override; + bool predict(const double dt, KalmanFilter & ekf) const; + bool measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) override; + bool measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); + bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool getTrackedObject( + const rclcpp::Time & time, + autoware_auto_perception_msgs::msg::TrackedObject & object) const override; + virtual ~ConstantTurnRateMotionTracker() {} +}; + +#endif // RADAR_OBJECT_TRACKER__TRACKER__MODEL__CONSTANT_TURN_RATE_MOTION_TRACKER_HPP_ diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp index 108e79c043ba0..77caa7a266481 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/model/linear_motion_tracker.hpp @@ -56,18 +56,25 @@ class LinearMotionTracker : public Tracker double p0_cov_vy; double p0_cov_ax; double p0_cov_ay; - } ekf_params_; + }; + static EkfParams ekf_params_; // limitation - double max_vx_; - double max_vy_; + static double max_vx_; + static double max_vy_; // rough tracking parameters float z_; float yaw_; // lpf parameter - double filter_tau_; // time constant of 1st order low pass filter - double filter_dt_; // sampling time of 1st order low pass filter + static double filter_tau_; // time constant of 1st order low pass filter + static double filter_dt_; // sampling time of 1st order low pass filter + + static bool is_initialized_; + static bool estimate_acc_; + static bool trust_yaw_input_; + static bool trust_twist_input_; + static bool use_polar_coordinate_in_measurement_noise_; private: struct BoundingBox @@ -89,13 +96,15 @@ class LinearMotionTracker : public Tracker const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, const std::string & tracker_param_file, const std::uint8_t & label); - void loadDefaultModelParameters(const std::string & path); + static void loadDefaultModelParameters(const std::string & path); bool predict(const rclcpp::Time & time) override; bool predict(const double dt, KalmanFilter & ekf) const; bool measure( const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_auto_perception_msgs::msg::DetectedObject & object); + bool measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform); bool measureWithShape(const autoware_auto_perception_msgs::msg::DetectedObject & object); bool getTrackedObject( const rclcpp::Time & time, diff --git a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp index 7da940912a253..70045e6b16a07 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/tracker/tracker.hpp @@ -15,6 +15,7 @@ #ifndef RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ #define RADAR_OBJECT_TRACKER__TRACKER__TRACKER_HPP_ +#include "model/constant_turn_rate_motion_tracker.hpp" #include "model/linear_motion_tracker.hpp" #include "model/tracker_base.hpp" diff --git a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml index e2d30c1b69d19..6e6813d3e40ff 100644 --- a/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml +++ b/perception/radar_object_tracker/launch/radar_object_tracker.launch.xml @@ -4,8 +4,9 @@ - + + @@ -17,6 +18,7 @@ + diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 6b02836e1f933..6d801302ab6c5 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -207,6 +207,7 @@ RadarObjectTrackerNode::RadarObjectTrackerNode(const rclcpp::NodeOptions & node_ // Parameters tracker_lifetime_ = declare_parameter("tracker_lifetime"); double publish_rate = declare_parameter("publish_rate"); + measurement_count_threshold_ = declare_parameter("measurement_count_threshold"); world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; tracker_config_directory_ = declare_parameter("tracking_config_directory"); @@ -363,6 +364,10 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( if (tracker == "linear_motion_tracker") { std::string config_file = tracker_config_directory_ + "linear_motion_tracker.yaml"; return std::make_shared(time, object, config_file, label); + } else if (tracker == "constant_turn_rate_motion_tracker") { + std::string config_file = + tracker_config_directory_ + "constant_turn_rate_motion_tracker.yaml"; + return std::make_shared(time, object, config_file, label); } else { // not implemented yet so put warning RCLCPP_WARN(get_logger(), "Tracker %s is not implemented yet", tracker.c_str()); @@ -530,8 +535,7 @@ void RadarObjectTrackerNode::sanitizeTracker( inline bool RadarObjectTrackerNode::shouldTrackerPublish( const std::shared_ptr tracker) const { - constexpr int measurement_count_threshold = 3; - if (tracker->getTotalMeasurementCount() < measurement_count_threshold) { + if (tracker->getTotalMeasurementCount() < measurement_count_threshold_) { return false; } return true; diff --git a/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp new file mode 100644 index 0000000000000..5815fe34a68a9 --- /dev/null +++ b/perception/radar_object_tracker/src/tracker/model/constant_turn_rate_motion_tracker.cpp @@ -0,0 +1,624 @@ +// Copyright 2020 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Yukihiro Saito +// + +#include "radar_object_tracker/tracker/model/constant_turn_rate_motion_tracker.hpp" + +#include "radar_object_tracker/utils/utils.hpp" + +#include +#include + +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#define EIGEN_MPL2_ONLY +#include +#include +#include +#include + +#include + +using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + +// init static member variables +bool ConstantTurnRateMotionTracker::is_initialized_ = false; +ConstantTurnRateMotionTracker::EkfParams ConstantTurnRateMotionTracker::ekf_params_; +double ConstantTurnRateMotionTracker::max_vx_; +double ConstantTurnRateMotionTracker::filter_tau_; +double ConstantTurnRateMotionTracker::filter_dt_; +bool ConstantTurnRateMotionTracker::assume_zero_yaw_rate_; +bool ConstantTurnRateMotionTracker::trust_yaw_input_; +bool ConstantTurnRateMotionTracker::trust_twist_input_; +bool ConstantTurnRateMotionTracker::use_polar_coordinate_in_measurement_noise_; + +ConstantTurnRateMotionTracker::ConstantTurnRateMotionTracker( + const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, + const std::string & tracker_param_file, const std::uint8_t & /*label*/) +: Tracker(time, object.classification), + logger_(rclcpp::get_logger("ConstantTurnRateMotionTracker")), + last_update_time_(time), + z_(object.kinematics.pose_with_covariance.pose.position.z) +{ + object_ = object; + + // load setting from yaml file + if (!is_initialized_) { + loadDefaultModelParameters(tracker_param_file); // currently not using label + is_initialized_ = true; + } + + // shape initialization + bounding_box_ = {0.5, 0.5, 1.7}; + cylinder_ = {0.3, 1.7}; + + // initialize X matrix and position + Eigen::MatrixXd X(ekf_params_.dim_x, 1); + X(IDX::X) = object.kinematics.pose_with_covariance.pose.position.x; + X(IDX::Y) = object.kinematics.pose_with_covariance.pose.position.y; + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + z_ = object.kinematics.pose_with_covariance.pose.position.z; + X(IDX::YAW) = yaw; + // radar object usually have twist + if (object.kinematics.has_twist) { + const auto v = object.kinematics.twist_with_covariance.twist.linear.x; + X(IDX::VX) = v; + } else { + X(IDX::VX) = 0.0; + } + // init turn rate + X(IDX::WZ) = 0.0; + + // initialize P matrix + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + + // create rotation matrix to rotate covariance matrix + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + // 2d rotation matrix + Eigen::Matrix2d R; + R << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; + + // covariance matrix in the target vehicle coordinate system + Eigen::Matrix2d P_xy_local; + P_xy_local << ekf_params_.p0_cov_x, 0.0, 0.0, ekf_params_.p0_cov_y; + + // Rotated covariance matrix + // covariance is rotated by 2D rotation matrix R + // P′=R P RT + Eigen::Matrix2d P_xy, P_v_xy; + + // Rotate the covariance matrix according to the vehicle yaw + // because p0_cov_x and y are in the vehicle coordinate system. + if (object.kinematics.has_position_covariance) { + P_xy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P_xy = R * P_xy_local * R.transpose(); + } else { + // rotate + P_xy = R * P_xy_local * R.transpose(); + } + // put value in P matrix + P.block<2, 2>(IDX::X, IDX::X) = P_xy; + + // covariance often written in object frame + if (object.kinematics.has_twist_covariance) { + const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + // const auto vy_cov = + // object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + P(IDX::VX, IDX::VX) = vx_cov; + } else { + P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx; + } + + P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw; + P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz; + + // init shape + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + bounding_box_ = { + object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + cylinder_ = {object.shape.dimensions.x, object.shape.dimensions.z}; + } + + ekf_.init(X, P); +} + +void ConstantTurnRateMotionTracker::loadDefaultModelParameters(const std::string & path) +{ + YAML::Node config = YAML::LoadFile(path); + // initialize ekf params + const float q_stddev_x = + config["default"]["ekf_params"]["process_noise_std"]["x"].as(); // [m] + const float q_stddev_y = + config["default"]["ekf_params"]["process_noise_std"]["y"].as(); // [m] + const float q_stddev_yaw = + config["default"]["ekf_params"]["process_noise_std"]["yaw"].as(); // [rad] + const float q_stddev_vx = + config["default"]["ekf_params"]["process_noise_std"]["vx"].as(); // [m/s] + const float q_stddev_wz = + config["default"]["ekf_params"]["process_noise_std"]["wz"].as(); // [m/s] + const float r_stddev_x = + config["default"]["ekf_params"]["measurement_noise_std"]["x"].as(); // [m] + const float r_stddev_y = + config["default"]["ekf_params"]["measurement_noise_std"]["y"].as(); // [m] + const float r_stddev_yaw = + config["default"]["ekf_params"]["measurement_noise_std"]["yaw"].as(); // [rad] + const float r_stddev_vx = + config["default"]["ekf_params"]["measurement_noise_std"]["vx"].as(); // [m/s] + const float p0_stddev_x = + config["default"]["ekf_params"]["initial_covariance_std"]["x"].as(); // [m/s] + const float p0_stddev_y = + config["default"]["ekf_params"]["initial_covariance_std"]["y"].as(); // [m/s] + const float p0_stddev_yaw = + config["default"]["ekf_params"]["initial_covariance_std"]["yaw"].as(); // [rad] + const float p0_stddev_vx = + config["default"]["ekf_params"]["initial_covariance_std"]["vx"].as(); // [m/(s)] + const float p0_stddev_wz = + config["default"]["ekf_params"]["initial_covariance_std"]["wz"].as(); // [rad/s] + assume_zero_yaw_rate_ = config["default"]["assume_zero_yaw_rate"].as(); // default false + trust_yaw_input_ = config["default"]["trust_yaw_input"].as(); // default false + trust_twist_input_ = config["default"]["trust_twist_input"].as(); // default false + use_polar_coordinate_in_measurement_noise_ = + config["default"]["use_polar_coordinate_in_measurement_noise"].as(); // default false + ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0); + ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0); + ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0); + ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); + ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0); + ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0); + ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0); + ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0); + ekf_params_.r_cov_vx = std::pow(r_stddev_vx, 2.0); + ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0); + ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0); + ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0); + ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0); + ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0); + + // lpf filter parameters + filter_tau_ = config["default"]["low_pass_filter"]["time_constant"].as(); // [s] + filter_dt_ = config["default"]["low_pass_filter"]["sampling_time"].as(); // [s] + + // limitation + // (TODO): this may be written in another yaml file based on classify result + const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] + max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] +} + +bool ConstantTurnRateMotionTracker::predict(const rclcpp::Time & time) +{ + const double dt = (time - last_update_time_).seconds(); + bool ret = predict(dt, ekf_); + if (ret) { + last_update_time_ = time; + } + return ret; +} + +bool ConstantTurnRateMotionTracker::predict(const double dt, KalmanFilter & ekf) const +{ + /* == Nonlinear model == + * + * x_{k+1} = x_k + vx_k * cos(yaw_k) * dt + * y_{k+1} = y_k + vx_k * sin(yaw_k) * dt + * yaw_{k+1} = yaw_k + (wz_k) * dt + * vx_{k+1} = vx_k + * wz_{k+1} = wz_k + * + */ + + /* == Linearized model == + * + * A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0] + * [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0] + * [ 0, 0, 1, 0, dt] + * [ 0, 0, 0, 1, 0] + * [ 0, 0, 0, 0, 1] + */ + + const double yaw_rate_coeff = assume_zero_yaw_rate_ ? 0.0 : 1.0; + + // X t + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + ekf.getX(X_t); + const auto x = X_t(IDX::X); + const auto y = X_t(IDX::Y); + const auto yaw = X_t(IDX::YAW); + const auto vx = X_t(IDX::VX); + const auto wz = X_t(IDX::WZ); + + // X t+1 + Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); + X_next_t(IDX::X) = x + vx * std::cos(yaw) * dt; + X_next_t(IDX::Y) = y + vx * std::sin(yaw) * dt; + X_next_t(IDX::YAW) = yaw + wz * dt * yaw_rate_coeff; + X_next_t(IDX::VX) = vx; + X_next_t(IDX::WZ) = wz * yaw_rate_coeff; + + // A: state transition matrix + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); + A(IDX::X, IDX::YAW) = -vx * std::sin(yaw) * dt; + A(IDX::Y, IDX::YAW) = vx * std::cos(yaw) * dt; + A(IDX::X, IDX::VX) = std::cos(yaw) * dt; + A(IDX::Y, IDX::VX) = std::sin(yaw) * dt; + A(IDX::YAW, IDX::WZ) = dt * yaw_rate_coeff; + + // Q: system noise + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + + // Rotate the covariance matrix according to the vehicle yaw + // because q_cov_x and y are in the vehicle coordinate system. + Eigen::MatrixXd Q_xy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Q_xy_global = Eigen::MatrixXd::Zero(2, 2); + Q_xy_local << ekf_params_.q_cov_x, 0.0, 0.0, ekf_params_.q_cov_y; + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(2, 2); + R << cos(yaw), -sin(yaw), sin(yaw), cos(yaw); + Q_xy_global = R * Q_xy_local * R.transpose(); + Q.block<2, 2>(IDX::X, IDX::X) = Q_xy_global; + + Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw; + Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx; + Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz; + + // may be unused + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); + Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); + + // call kalman filter library + if (!ekf.predict(X_next_t, A, Q)) { + RCLCPP_WARN(logger_, "Cannot predict"); + } + + return true; +} + +bool ConstantTurnRateMotionTracker::measureWithPose( + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) +{ + // Observation pattern will be: + // 1. x, y, vx, vy + // 2. x, y + // 3. vx, vy (Do not consider this right now) + // + // We handle this measurements by stacking observation matrix, measurement vector and measurement + // covariance + // - observation matrix: C + // - measurement vector : Y + // - measurement covariance: R + + // get current state + Eigen::MatrixXd X(ekf_params_.dim_x, 1); + ekf_.getX(X); + const auto yaw_state = X(IDX::YAW); + + // rotation matrix + Eigen::Matrix2d RotationYaw; + if (trust_yaw_input_) { + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + } else { + RotationYaw << std::cos(yaw_state), -std::sin(yaw_state), std::sin(yaw_state), + std::cos(yaw_state); + } + const auto base_link_yaw = tf2::getYaw(self_transform.rotation); + Eigen::Matrix2d RotationBaseLink; + RotationBaseLink << std::cos(base_link_yaw), -std::sin(base_link_yaw), std::sin(base_link_yaw), + std::cos(base_link_yaw); + + // depth from base_link to object + const auto dx = + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x; + const auto dy = + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y; + Eigen::Vector2d pose_diff_in_map(dx, dy); + Eigen::Vector2d pose_diff_in_base_link = RotationBaseLink * pose_diff_in_map; + const auto depth = abs(pose_diff_in_base_link(0)); + + // gather matrices as vector + std::vector C_list; + std::vector Y_list; + std::vector R_block_list; + + // 1. add position measurement + const bool enable_position_measurement = true; // assume position is always measured + if (enable_position_measurement) { + Eigen::MatrixXd Cxy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); + Cxy(0, IDX::X) = 1; + Cxy(1, IDX::Y) = 1; + C_list.push_back(Cxy); + + Eigen::MatrixXd Yxy = Eigen::MatrixXd::Zero(2, 1); + Yxy << object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y; + Y_list.push_back(Yxy); + + // covariance need to be rotated since it is in the vehicle coordinate system + Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); + if (!object.kinematics.has_position_covariance) { + // switch noise covariance in polar coordinate or cartesian coordinate + const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ + ? depth * depth * ekf_params_.r_cov_x + : ekf_params_.r_cov_y; + Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate + Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); + } else { + Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], + object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], + object.kinematics.pose_with_covariance + .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); + } + R_block_list.push_back(Rxy); + } + + // 2. add yaw measurement + const bool object_has_orientation = object.kinematics.orientation_availability > + 0; // 0: not available, 1: sign unknown, 2: available + const bool enable_yaw_measurement = trust_yaw_input_ && object_has_orientation; + + if (enable_yaw_measurement) { + Eigen::MatrixXd Cyaw = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); + Cyaw(0, IDX::YAW) = 1; + C_list.push_back(Cyaw); + + Eigen::MatrixXd Yyaw = Eigen::MatrixXd::Zero(1, 1); + const auto yaw = [&] { + auto obj_yaw = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); + while (M_PI_2 <= yaw_state - obj_yaw) { + obj_yaw = obj_yaw + M_PI; + } + while (M_PI_2 <= obj_yaw - yaw_state) { + obj_yaw = obj_yaw - M_PI; + } + return obj_yaw; + }(); + + Yyaw << yaw; + Y_list.push_back(Yyaw); + + Eigen::MatrixXd Ryaw = Eigen::MatrixXd::Zero(1, 1); + Ryaw << ekf_params_.r_cov_yaw; + R_block_list.push_back(Ryaw); + } + + // 3. add linear velocity measurement + const bool enable_velocity_measurement = object.kinematics.has_twist && trust_twist_input_; + if (enable_velocity_measurement) { + Eigen::MatrixXd C_vx = Eigen::MatrixXd::Zero(1, ekf_params_.dim_x); + C_vx(0, IDX::VX) = 1; + C_list.push_back(C_vx); + + // measure absolute velocity + Eigen::MatrixXd Vx = Eigen::MatrixXd::Zero(1, 1); + Vx << object.kinematics.twist_with_covariance.twist.linear.x; + + Eigen::Matrix2d R_vx = Eigen::MatrixXd::Zero(1, 1); + if (!object.kinematics.has_twist_covariance) { + R_vx << ekf_params_.r_cov_vx; + } else { + R_vx << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; + } + R_block_list.push_back(R_vx); + } + + // 4. sum up matrices + const auto row_number = std::accumulate( + C_list.begin(), C_list.end(), 0, + [](const auto & sum, const auto & C) { return sum + C.rows(); }); + if (row_number == 0) { + RCLCPP_WARN(logger_, "No measurement is available"); + return false; + } + + // stacking matrices vertically or diagonally + const auto C = utils::stackMatricesVertically(C_list); + const auto Y = utils::stackMatricesVertically(Y_list); + const auto R = utils::stackMatricesDiagonally(R_block_list); + + // 4. EKF update + if (!ekf_.update(Y, C, R)) { + RCLCPP_WARN(logger_, "Cannot update"); + } + + // 5. normalize: limit vx + { + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + Eigen::MatrixXd P_t(ekf_params_.dim_x, ekf_params_.dim_x); + ekf_.getX(X_t); + ekf_.getP(P_t); + if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) { + X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_; + } + ekf_.init(X_t, P_t); + } + + // 6. Filter z + // first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; + return true; +} + +bool ConstantTurnRateMotionTracker::measureWithShape( + const autoware_auto_perception_msgs::msg::DetectedObject & object) +{ + // just use first order low pass filter + const float gain = filter_tau_ / (filter_tau_ + filter_dt_); + + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + bounding_box_.length = gain * bounding_box_.length + (1.0 - gain) * object.shape.dimensions.x; + bounding_box_.width = gain * bounding_box_.width + (1.0 - gain) * object.shape.dimensions.y; + bounding_box_.height = gain * bounding_box_.height + (1.0 - gain) * object.shape.dimensions.z; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + cylinder_.width = gain * cylinder_.width + (1.0 - gain) * object.shape.dimensions.x; + cylinder_.height = gain * cylinder_.height + (1.0 - gain) * object.shape.dimensions.z; + } else { + return false; + } + + return true; +} + +bool ConstantTurnRateMotionTracker::measure( + const autoware_auto_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const geometry_msgs::msg::Transform & self_transform) +{ + const auto & current_classification = getClassification(); + object_ = object; + if (object_recognition_utils::getHighestProbLabel(object.classification) == Label::UNKNOWN) { + setClassification(current_classification); + } + + if (0.01 /*10msec*/ < std::fabs((time - last_update_time_).seconds())) { + RCLCPP_WARN( + logger_, "There is a large gap between predicted time and measurement time. (%f)", + (time - last_update_time_).seconds()); + } + + measureWithPose(object, self_transform); + measureWithShape(object); + + // (void)self_transform; // currently do not use self vehicle position + return true; +} + +bool ConstantTurnRateMotionTracker::getTrackedObject( + const rclcpp::Time & time, autoware_auto_perception_msgs::msg::TrackedObject & object) const +{ + object = object_recognition_utils::toTrackedObject(object_); + object.object_id = getUUID(); + object.classification = getClassification(); + + // predict kinematics + KalmanFilter tmp_ekf_for_no_update = ekf_; + const double dt = (time - last_update_time_).seconds(); + if (0.001 /*1msec*/ < dt) { + predict(dt, tmp_ekf_for_no_update); + } + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state + Eigen::MatrixXd P(ekf_params_.dim_x, ekf_params_.dim_x); // predicted state + tmp_ekf_for_no_update.getX(X_t); + tmp_ekf_for_no_update.getP(P); + + auto & pose_with_cov = object.kinematics.pose_with_covariance; + auto & twist_with_cov = object.kinematics.twist_with_covariance; + // rotation matrix with yaw_ + Eigen::Matrix2d R_yaw = Eigen::Matrix2d::Zero(); + R_yaw << std::cos(X_t(IDX::YAW)), -std::sin(X_t(IDX::YAW)), std::sin(X_t(IDX::YAW)), + std::cos(X_t(IDX::YAW)); + const Eigen::Matrix2d R_yaw_inv = R_yaw.transpose(); + + // position + pose_with_cov.pose.position.x = X_t(IDX::X); + pose_with_cov.pose.position.y = X_t(IDX::Y); + pose_with_cov.pose.position.z = z_; + // quaternion + { + double roll, pitch, yaw; + tf2::Quaternion original_quaternion; + tf2::fromMsg(object_.kinematics.pose_with_covariance.pose.orientation, original_quaternion); + tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); + tf2::Quaternion filtered_quaternion; + filtered_quaternion.setRPY(roll, pitch, X_t(IDX::YAW)); + pose_with_cov.pose.orientation.x = filtered_quaternion.x(); + pose_with_cov.pose.orientation.y = filtered_quaternion.y(); + pose_with_cov.pose.orientation.z = filtered_quaternion.z(); + pose_with_cov.pose.orientation.w = filtered_quaternion.w(); + if (trust_yaw_input_) { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } + } + // twist + // twist need to converted to local coordinate + const auto vx = X_t(IDX::VX); + twist_with_cov.twist.linear.x = vx; + + // ===== covariance transformation ===== + // since covariance in EKF is in map coordinate and output should be in object coordinate, + // we need to transform covariance + Eigen::Matrix2d P_xy_map; + P_xy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); + + // rotate covariance with -yaw + Eigen::Matrix2d P_xy = R_yaw_inv * P_xy_map * R_yaw_inv.transpose(); + + // position covariance + constexpr double no_info_cov = 1e9; // no information + constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative + constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative + + pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + pose_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = yaw_cov; + + // twist covariance + constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative + constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative + + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = 0.0; + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = 0.0; + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; + twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; + + // set shape + if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + object.shape.dimensions.x = bounding_box_.length; + object.shape.dimensions.y = bounding_box_.width; + object.shape.dimensions.z = bounding_box_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + object.shape.dimensions.x = cylinder_.width; + object.shape.dimensions.y = cylinder_.width; + object.shape.dimensions.z = cylinder_.height; + } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + const auto origin_yaw = tf2::getYaw(object_.kinematics.pose_with_covariance.pose.orientation); + const auto ekf_pose_yaw = tf2::getYaw(pose_with_cov.pose.orientation); + object.shape.footprint = + tier4_autoware_utils::rotatePolygon(object.shape.footprint, origin_yaw - ekf_pose_yaw); + } + + return true; +} diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index f6814ceb8c246..8369d04fbe225 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -44,9 +44,21 @@ using Label = autoware_auto_perception_msgs::msg::ObjectClassification; +// initialize static parameter +bool LinearMotionTracker::is_initialized_ = false; +LinearMotionTracker::EkfParams LinearMotionTracker::ekf_params_; +double LinearMotionTracker::max_vx_; +double LinearMotionTracker::max_vy_; +double LinearMotionTracker::filter_tau_; +double LinearMotionTracker::filter_dt_; +bool LinearMotionTracker::estimate_acc_; +bool LinearMotionTracker::trust_yaw_input_; +bool LinearMotionTracker::trust_twist_input_; +bool LinearMotionTracker::use_polar_coordinate_in_measurement_noise_; + LinearMotionTracker::LinearMotionTracker( const rclcpp::Time & time, const autoware_auto_perception_msgs::msg::DetectedObject & object, - const std::string & /*tracker_param_file*/, const std::uint8_t & /*label*/) + const std::string & tracker_param_file, const std::uint8_t & /*label*/) : Tracker(time, object.classification), logger_(rclcpp::get_logger("LinearMotionTracker")), last_update_time_(time), @@ -55,11 +67,13 @@ LinearMotionTracker::LinearMotionTracker( object_ = object; // load setting from yaml file - const std::string default_setting_file = - ament_index_cpp::get_package_share_directory("radar_object_tracker") + - "/config/tracking/linear_motion_tracker.yaml"; - loadDefaultModelParameters(default_setting_file); - // loadModelParameters(tracker_param_file, label); + if (!is_initialized_) { + loadDefaultModelParameters(tracker_param_file); + is_initialized_ = true; + } + // shape initialization + bounding_box_ = {0.5, 0.5, 1.7}; + cylinder_ = {0.3, 1.7}; // initialize X matrix and position Eigen::MatrixXd X(ekf_params_.dim_x, 1); @@ -193,6 +207,12 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) config["default"]["ekf_params"]["initial_covariance_std"]["ax"].as(); // [m/(s*s)] const float p0_stddev_ay = config["default"]["ekf_params"]["initial_covariance_std"]["ay"].as(); // [m/(s*s)] + estimate_acc_ = config["default"]["ekf_params"]["estimate_acc"].as(); + trust_yaw_input_ = config["default"]["trust_yaw_input"].as(false); // default false + trust_twist_input_ = config["default"]["trust_twist_input"].as(false); // default false + use_polar_coordinate_in_measurement_noise_ = + config["default"]["use_polar_coordinate_in_measurement_noise"].as( + false); // default false ekf_params_.q_cov_ax = std::pow(q_stddev_ax, 2.0); ekf_params_.q_cov_ay = std::pow(q_stddev_ay, 2.0); ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0); @@ -219,11 +239,6 @@ void LinearMotionTracker::loadDefaultModelParameters(const std::string & path) const float max_speed_kmph = config["default"]["limit"]["max_speed"].as(); // [km/h] max_vx_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [m/s] max_vy_ = tier4_autoware_utils::kmph2mps(max_speed_kmph); // [rad/s] - - // shape initialization - // (TODO): this may be written in another yaml file based on classify result - bounding_box_ = {0.5, 0.5, 1.7}; - cylinder_ = {0.3, 1.7}; } bool LinearMotionTracker::predict(const rclcpp::Time & time) @@ -257,6 +272,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const * 0, 0, 0, 0, 1, 0, * 0, 0, 0, 0, 0, 1] */ + // estimate acc + const double acc_coeff = estimate_acc_ ? 1.0 : 0.0; // X t Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state @@ -270,10 +287,10 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const // X t+1 Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); - X_next_t(IDX::X) = x + vx * dt + 0.5 * ax * dt * dt; - X_next_t(IDX::Y) = y + vy * dt + 0.5 * ay * dt * dt; - X_next_t(IDX::VX) = vx + ax * dt; - X_next_t(IDX::VY) = vy + ay * dt; + X_next_t(IDX::X) = x + vx * dt + 0.5 * ax * dt * dt * acc_coeff; + X_next_t(IDX::Y) = y + vy * dt + 0.5 * ay * dt * dt * acc_coeff; + X_next_t(IDX::VX) = vx + ax * dt * acc_coeff; + X_next_t(IDX::VY) = vy + ay * dt * acc_coeff; X_next_t(IDX::AX) = ax; X_next_t(IDX::AY) = ay; @@ -281,10 +298,10 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x); A(IDX::X, IDX::VX) = dt; A(IDX::Y, IDX::VY) = dt; - A(IDX::X, IDX::AX) = 0.5 * dt * dt; - A(IDX::Y, IDX::AY) = 0.5 * dt * dt; - A(IDX::VX, IDX::AX) = dt; - A(IDX::VY, IDX::AY) = dt; + A(IDX::X, IDX::AX) = 0.5 * dt * dt * acc_coeff; + A(IDX::Y, IDX::AY) = 0.5 * dt * dt * acc_coeff; + A(IDX::VX, IDX::AX) = dt * acc_coeff; + A(IDX::VY, IDX::AY) = dt * acc_coeff; // Q: system noise Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x); @@ -333,7 +350,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const } bool LinearMotionTracker::measureWithPose( - const autoware_auto_perception_msgs::msg::DetectedObject & object) + const autoware_auto_perception_msgs::msg::DetectedObject & object, + const geometry_msgs::msg::Transform & self_transform) { // Observation pattern will be: // 1. x, y, vx, vy @@ -348,8 +366,25 @@ bool LinearMotionTracker::measureWithPose( // rotation matrix Eigen::Matrix2d RotationYaw; - const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + if (trust_yaw_input_) { + const auto yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + RotationYaw << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw); + } else { + RotationYaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); + } + const auto base_link_yaw = tf2::getYaw(self_transform.rotation); + Eigen::Matrix2d RotationBaseLink; + RotationBaseLink << std::cos(base_link_yaw), -std::sin(base_link_yaw), std::sin(base_link_yaw), + std::cos(base_link_yaw); + + // depth from base_link to object + const auto dx = + object.kinematics.pose_with_covariance.pose.position.x - self_transform.translation.x; + const auto dy = + object.kinematics.pose_with_covariance.pose.position.y - self_transform.translation.y; + Eigen::Vector2d pose_diff_in_map(dx, dy); + Eigen::Vector2d pose_diff_in_base_link = RotationBaseLink * pose_diff_in_map; + const auto depth = abs(pose_diff_in_base_link(0)); // gather matrices as vector std::vector C_list; @@ -371,21 +406,27 @@ bool LinearMotionTracker::measureWithPose( // covariance need to be rotated since it is in the vehicle coordinate system Eigen::MatrixXd Rxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_position_covariance) { - Rxy_local << ekf_params_.r_cov_x, 0, 0, ekf_params_.r_cov_y; + // switch noise covariance in polar coordinate or cartesian coordinate + const auto r_cov_y = use_polar_coordinate_in_measurement_noise_ + ? depth * depth * ekf_params_.r_cov_x + : ekf_params_.r_cov_y; + Rxy_local << ekf_params_.r_cov_x, 0, 0, r_cov_y; // xy in base_link coordinate + Rxy = RotationBaseLink * Rxy_local * RotationBaseLink.transpose(); } else { Rxy_local << object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X], object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y], object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_X], - object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + object.kinematics.pose_with_covariance + .covariance[utils::MSG_COV_IDX::Y_Y]; // xy in vehicle coordinate + Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); } - Eigen::MatrixXd Rxy = Eigen::MatrixXd::Zero(2, 2); - Rxy = RotationYaw * Rxy_local * RotationYaw.transpose(); R_block_list.push_back(Rxy); } // 2. add linear velocity measurement - const bool enable_velocity_measurement = object.kinematics.has_twist; + const bool enable_velocity_measurement = object.kinematics.has_twist && trust_twist_input_; if (enable_velocity_measurement) { Eigen::MatrixXd C_vx_vy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); C_vx_vy(0, IDX::VX) = 1; @@ -401,14 +442,15 @@ bool LinearMotionTracker::measureWithPose( Y_list.push_back(Vxy); Eigen::Matrix2d R_v_xy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_twist_covariance) { R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; + R_v_xy = RotationBaseLink * R_v_xy_local * RotationBaseLink.transpose(); } else { R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], 0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); } - Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); - R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); R_block_list.push_back(R_v_xy); } @@ -453,8 +495,16 @@ bool LinearMotionTracker::measureWithPose( // first order low pass filter const float gain = filter_tau_ / (filter_tau_ + filter_dt_); z_ = gain * z_ + (1.0 - gain) * object.kinematics.pose_with_covariance.pose.position.z; - yaw_ = gain * yaw_ + (1.0 - gain) * yaw; - + // get yaw from twist atan + Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); + ekf_.getX(X_t); + const auto twist_yaw = + std::atan2(X_t(IDX::VY), X_t(IDX::VX)); // calc from lateral and longitudinal velocity + if (trust_yaw_input_) { + yaw_ = gain * yaw_ + (1.0 - gain) * twist_yaw; + } else { + yaw_ = twist_yaw; + } return true; } @@ -494,10 +544,10 @@ bool LinearMotionTracker::measure( (time - last_update_time_).seconds()); } - measureWithPose(object); + measureWithPose(object, self_transform); measureWithShape(object); - (void)self_transform; // currently do not use self vehicle position + // (void)self_transform; // currently do not use self vehicle position return true; } @@ -543,8 +593,13 @@ bool LinearMotionTracker::getTrackedObject( pose_with_cov.pose.orientation.y = filtered_quaternion.y(); pose_with_cov.pose.orientation.z = filtered_quaternion.z(); pose_with_cov.pose.orientation.w = filtered_quaternion.w(); - object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + if (trust_yaw_input_) { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else { + object.kinematics.orientation_availability = + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } } // twist // twist need to converted to local coordinate From 2ee44a36161ef7598208ba45b516b35d334aba26 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Mon, 30 Oct 2023 21:34:05 +0900 Subject: [PATCH 201/206] feat(avoidance): improve force avoidance judge logic in order to suppress unnecessary avoidance path (#5441) * refactor(avoidance): cleanup force avoidance params Signed-off-by: satoshi-ota * feat(avoidance): improve force avoidance judgement Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 17 ++- .../utils/avoidance/avoidance_module_data.hpp | 3 + .../utils/avoidance/utils.hpp | 3 + .../behavior_path_planner/utils/utils.hpp | 2 + .../src/scene_module/avoidance/manager.cpp | 24 ++-- .../src/scene_module/lane_change/manager.cpp | 24 ++-- .../src/utils/avoidance/utils.cpp | 117 ++++++++++++------ .../behavior_path_planner/src/utils/utils.cpp | 11 ++ 8 files changed, 140 insertions(+), 61 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 3d0a334cedf84..b0f736916ebda 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -9,7 +9,6 @@ # avoidance module common setting enable_bound_clipping: false - enable_force_avoidance_for_stopped_vehicle: false enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false enable_cancel_maneuver: false @@ -120,11 +119,6 @@ # For target object filtering target_filtering: - # params for avoidance of not-parked objects - threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] - object_ignore_section_traffic_light_in_front_distance: 30.0 # [m] - object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] - object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range object_check_goal_distance: 20.0 # [m] # filtering parking objects @@ -141,6 +135,17 @@ max_forward_distance: 150.0 # [m] backward_distance: 10.0 # [m] + # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked. + force_avoidance: + enable: false # [-] + time_threshold: 1.0 # [s] + ignore_area: + traffic_light: + front_distance: 100.0 # [m] + crosswalk: + front_distance: 30.0 # [m] + behind_distance: 30.0 # [m] + # For safety check safety_check: # safety check configuration diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index e881c8a0d3b18..7976399a7ed40 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -381,6 +381,9 @@ struct ObjectData // avoidance target // is stoppable under the constraints bool is_stoppable{false}; + // is within intersection area + bool is_within_intersection{false}; + // unavoidable reason std::string reason{""}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 0237fb458ea0b..b8b4efb7aed57 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -39,6 +39,9 @@ bool isWithinCrosswalk( const ObjectData & object, const std::shared_ptr & overall_graphs); +bool isWithinIntersection( + const ObjectData & object, const std::shared_ptr & route_handler); + bool isTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 38c2620096ca2..bfa83c43b061c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -330,6 +330,8 @@ std::optional getSignedDistanceFromBoundary( Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet); +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon); + std::vector getTargetLaneletPolygons( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 7988239e68071..33c6982f99fd9 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -43,8 +43,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "resample_interval_for_output"); p.enable_bound_clipping = getOrDeclareParameter(*node, ns + "enable_bound_clipping"); p.enable_cancel_maneuver = getOrDeclareParameter(*node, ns + "enable_cancel_maneuver"); - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); p.enable_yield_maneuver = getOrDeclareParameter(*node, ns + "enable_yield_maneuver"); p.enable_yield_maneuver_during_shifting = getOrDeclareParameter(*node, ns + "enable_yield_maneuver_during_shifting"); @@ -108,14 +106,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( - *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_crosswalk_in_front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -128,6 +118,20 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + { std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 78e05c940a814..ee7b45de09e93 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -275,8 +275,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = getOrDeclareParameter(*node, ns + "resample_interval_for_output"); - p.enable_force_avoidance_for_stopped_vehicle = - getOrDeclareParameter(*node, ns + "enable_force_avoidance_for_stopped_vehicle"); } // target object @@ -320,14 +318,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // target filtering { std::string ns = "avoidance.target_filtering."; - p.threshold_time_force_avoidance_for_stopped_vehicle = getOrDeclareParameter( - *node, ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_section_traffic_light_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_traffic_light_in_front_distance"); - p.object_ignore_section_crosswalk_in_front_distance = getOrDeclareParameter( - *node, ns + "object_ignore_section_crosswalk_in_front_distance"); - p.object_ignore_section_crosswalk_behind_distance = - getOrDeclareParameter(*node, ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -340,6 +330,20 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } + { + std::string ns = "avoidance.target_filtering.force_avoidance."; + p.enable_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "enable"); + p.threshold_time_force_avoidance_for_stopped_vehicle = + getOrDeclareParameter(*node, ns + "time_threshold"); + p.object_ignore_section_traffic_light_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.traffic_light.front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + getOrDeclareParameter(*node, ns + "ignore_area.crosswalk.behind_distance"); + } + { std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 725b528abad51..54a1c0c649916 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -292,6 +292,81 @@ bool isWithinCrosswalk( return false; } +bool isWithinIntersection( + const ObjectData & object, const std::shared_ptr & route_handler) +{ + const std::string id = object.overhang_lanelet.attributeOr("intersection_area", "else"); + if (id == "else") { + return false; + } + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object.object); + + const auto polygon = route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(id.c_str())); + + return boost::geometry::within( + object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); +} + +bool isForceAvoidanceTarget( + ObjectData & object, const lanelet::ConstLanelets & extend_lanelets, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (!parameters->enable_force_avoidance_for_stopped_vehicle) { + return false; + } + + const auto stop_time_longer_than_threshold = + object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; + + if (!stop_time_longer_than_threshold) { + return false; + } + + if (object.is_within_intersection) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is in the intersection area."); + return false; + } + + const auto rh = planner_data->route_handler; + + if ( + !!rh->getRoutingGraphPtr()->right(object.overhang_lanelet) && + !!rh->getRoutingGraphPtr()->left(object.overhang_lanelet)) { + RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane."); + return false; + } + + const auto & ego_pose = planner_data->self_odometry->pose.pose; + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + + // force avoidance for stopped vehicle + bool not_parked_object = true; + + // check traffic light + const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); + { + not_parked_object = + to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; + } + + // check crosswalk + const auto to_crosswalk = + utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - + object.longitudinal; + { + const auto stop_for_crosswalk = + to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && + to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; + not_parked_object = not_parked_object || stop_for_crosswalk; + } + + object.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); + + return !not_parked_object; +} + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) { @@ -1127,42 +1202,14 @@ void filterTargetObjects( continue; } - // from here condition check for vehicle type objects. + o.is_within_intersection = isWithinIntersection(o, rh); - const auto stop_time_longer_than_threshold = - o.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle; - - if (stop_time_longer_than_threshold && parameters->enable_force_avoidance_for_stopped_vehicle) { - // force avoidance for stopped vehicle - bool not_parked_object = true; - - // check traffic light - const auto to_traffic_light = - utils::getDistanceToNextTrafficLight(object_pose, extend_lanelets); - { - not_parked_object = - to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; - } - - // check crosswalk - const auto to_crosswalk = - utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - - o.longitudinal; - { - const auto stop_for_crosswalk = - to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && - to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; - not_parked_object = not_parked_object || stop_for_crosswalk; - } - - o.to_stop_factor_distance = std::min(to_traffic_light, to_crosswalk); - - if (!not_parked_object) { - o.last_seen = now; - o.avoid_margin = avoid_margin; - data.target_objects.push_back(o); - continue; - } + // from here condition check for vehicle type objects. + if (isForceAvoidanceTarget(o, extend_lanelets, planner_data, parameters)) { + o.last_seen = now; + o.avoid_margin = avoid_margin; + data.target_objects.push_back(o); + continue; } // Object is on center line -> ignore. diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 3ab20e7f76718..37a628f54607e 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -2708,6 +2708,17 @@ Polygon2d toPolygon2d(const lanelet::ConstLanelet & lanelet) : tier4_autoware_utils::inverseClockwise(polygon); } +Polygon2d toPolygon2d(const lanelet::BasicPolygon2d & polygon) +{ + Polygon2d ret; + for (const auto & p : polygon) { + ret.outer().emplace_back(p.x(), p.y()); + } + ret.outer().push_back(ret.outer().front()); + + return tier4_autoware_utils::isClockwise(ret) ? ret : tier4_autoware_utils::inverseClockwise(ret); +} + std::vector getTargetLaneletPolygons( const lanelet::PolygonLayer & map_polygons, lanelet::ConstLanelets & lanelets, const Pose & pose, const double check_length, const std::string & target_type) From 49af2650accb154985e413df9b93f2a531811407 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 31 Oct 2023 10:48:04 +0900 Subject: [PATCH 202/206] fix(behavior_path_planner): fix turn signal endless loop (#5444) Signed-off-by: kosuke55 --- .../src/turn_signal_decider.cpp | 24 ++++--------------- 1 file changed, 5 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 3fe30b11e3a57..d1b8bdc806bf4 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -165,30 +165,16 @@ boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo const std::string lane_attribute = current_lane.attributeOr("turn_direction", std::string("none")); - // check next lanes - auto next_lanes = route_handler.getNextLanelets(current_lane); - if (next_lanes.empty()) { + // check next lane has the same attribute + lanelet::ConstLanelet next_lane{}; + if (!route_handler.getNextLaneletWithinRoute(current_lane, &next_lane)) { break; } - - // filter next lanes with same attribute in the path - std::vector next_lanes_in_path{}; - std::copy_if( - next_lanes.begin(), next_lanes.end(), std::back_inserter(next_lanes_in_path), - [&](const auto & next_lane) { - const bool is_in_unique_ids = - std::find(unique_lane_ids.begin(), unique_lane_ids.end(), next_lane.id()) != - unique_lane_ids.end(); - const bool has_same_attribute = - next_lane.attributeOr("turn_direction", std::string("none")) == lane_attribute; - return is_in_unique_ids && has_same_attribute; - }); - if (next_lanes_in_path.empty()) { + if (next_lane.attributeOr("turn_direction", std::string("none")) != lane_attribute) { break; } - // assume that the next lane in the path is only one - current_lane = next_lanes_in_path.front(); + current_lane = next_lane; } if (!combined_lane_elems.empty()) { From a194589eb0983ebbc5b77d5eebd6990e1ec8eb0f Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 31 Oct 2023 14:05:32 +0900 Subject: [PATCH 203/206] chore(intersection): add debug plotter (#5432) Signed-off-by: Mamoru Sobue --- .cspell-partial.json | 2 +- .../CMakeLists.txt | 5 + .../config/intersection.param.yaml | 3 + .../scripts/ttc.py | 287 ++++++++++++++++++ .../src/manager.cpp | 1 + .../src/scene_intersection.cpp | 58 +++- .../src/scene_intersection.hpp | 7 + .../src/util.cpp | 31 +- .../src/util.hpp | 12 +- 9 files changed, 393 insertions(+), 13 deletions(-) create mode 100755 planning/behavior_velocity_intersection_module/scripts/ttc.py diff --git a/.cspell-partial.json b/.cspell-partial.json index 13849ef298019..e231eddd712ea 100644 --- a/.cspell-partial.json +++ b/.cspell-partial.json @@ -5,5 +5,5 @@ "perception/bytetrack/lib/**" ], "ignoreRegExpList": [], - "words": ["dltype", "tvmgen"] + "words": ["dltype", "tvmgen", "quantizer", "imageio", "mimsave"] } diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/behavior_velocity_intersection_module/CMakeLists.txt index 8ff78dac06716..9e7eb196cd0c1 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/behavior_velocity_intersection_module/CMakeLists.txt @@ -20,3 +20,8 @@ target_link_libraries(${PROJECT_NAME} ) ament_auto_package(INSTALL_TO_SHARE config) + +install(PROGRAMS + scripts/ttc.py + DESTINATION lib/${PROJECT_NAME} +) diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 82a5f65622d0b..adbc7b3e087d6 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -83,6 +83,9 @@ attention_lane_curvature_calculation_ds: 0.5 static_occlusion_with_traffic_light_timeout: 3.5 + debug: + ttc: [0] + enable_rtc: intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval intersection_to_occlusion: false diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py new file mode 100755 index 0000000000000..e4633981570d1 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py @@ -0,0 +1,287 @@ +#!/usr/bin/env python3 + +# Copyright 2023 TIER IV, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import argparse +from dataclasses import dataclass +from itertools import cycle +import math +from threading import Lock +import time + +import imageio +import matplotlib +import matplotlib.pyplot as plt +import numpy as np +import rclpy +from rclpy.node import Node +from tier4_debug_msgs.msg import Float64MultiArrayStamped + +matplotlib.use("TKAgg") + + +@dataclass +class NPC: + x: float + y: float + th: float + width: float + height: float + speed: float + dangerous: bool + ref_object_enter_time: float + ref_object_exit_time: float + collision_start_time: float + collision_start_dist: float + collision_end_time: float + collision_end_dist: float + pred_x: list[float] + pred_y: list[float] + + def __init__(self, data: list[float]): + self.x = data[0] + self.y = data[1] + self.th = data[2] + self.width = data[3] + self.height = data[4] + self.speed = data[5] + self.dangerous = bool(int(data[6])) + self.ref_object_enter_time = data[7] + self.ref_object_exit_time = data[8] + self.collision_start_time = data[9] + self.collision_start_dist = data[10] + self.collision_end_time = data[11] + self.collision_end_dist = data[12] + self.first_collision_x = data[13] + self.first_collision_y = data[14] + self.last_collision_x = data[15] + self.last_collision_y = data[16] + self.pred_x = data[17:58:2] + self.pred_y = data[18:58:2] + + +class TTCVisualizer(Node): + def __init__(self, args): + super().__init__("ttc_visualizer") + self.ttc_dist_pub = self.create_subscription( + Float64MultiArrayStamped, + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/ego_ttc", + self.on_ego_ttc, + 1, + ) + self.ttc_time_pub = self.create_subscription( + Float64MultiArrayStamped, + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/object_ttc", + self.on_object_ttc, + 1, + ) + self.args = args + self.lane_id = args.lane_id + self.ego_ttc_data = None + self.object_ttc_data = None + self.npc_vehicles = [] + self.images = [] + self.last_sub = time.time() + + self.plot_timer = self.create_timer(0.2, self.on_plot_timer) + self.fig = plt.figure(figsize=(13, 6)) + self.ttc_ax = self.fig.add_subplot(1, 2, 1) + self.ttc_vel_ax = self.ttc_ax.twinx() + self.world_ax = self.fig.add_subplot(1, 2, 2) + self.lock = Lock() + self.color_list = [ + "#e41a1c", + "#377eb8", + "#4daf4a", + "#984ea3", + "#ff7f00", + "#ffff33", + "#a65628", + "#f781bf", + ] + plt.ion() + plt.show(block=False) + + def plot_ttc(self): + self.ttc_ax.cla() + self.ttc_vel_ax.cla() + + n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size) + ego_ttc_time = self.ego_ttc_data.data[n_ttc_data : 2 * n_ttc_data] + ego_ttc_dist = self.ego_ttc_data.data[2 * n_ttc_data : 3 * n_ttc_data] + + self.ttc_ax.grid() + self.ttc_ax.set_xlabel("ego time") + self.ttc_ax.set_ylabel("ego dist") + time_dist_plot = self.ttc_ax.plot(ego_ttc_time, ego_ttc_dist, label="time-dist", c="orange") + self.ttc_ax.set_xlim(min(ego_ttc_time) - 2.0, max(ego_ttc_time) + 3.0) + self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0) + for npc, color in zip(self.npc_vehicles, cycle(self.color_list)): + t0, t1 = npc.collision_start_time, npc.collision_end_time + d0, d1 = npc.collision_start_dist, npc.collision_end_dist + self.ttc_ax.fill( + [t0, t0, t1, t1, 0, 0], + [d0, 0, 0, d1, d1, d0], + c=color, + alpha=0.2, + ) + + dd = [d1 - d0 for d0, d1 in zip(ego_ttc_dist, ego_ttc_dist[1:])] + dt = [t1 - t0 for t0, t1 in zip(ego_ttc_time, ego_ttc_time[1:])] + v = [d / t for d, t in zip(dd, dt)] + self.ttc_vel_ax.yaxis.set_label_position("right") + self.ttc_vel_ax.set_ylabel("ego velocity") + self.ttc_vel_ax.set_ylim(0.0, max(v) + 1.0) + time_velocity_plot = self.ttc_vel_ax.plot(ego_ttc_time[1:], v, label="time-v", c="red") + + lines = time_dist_plot + time_velocity_plot + labels = [line.get_label() for line in lines] + self.ttc_ax.legend(lines, labels, loc="upper left") + + def plot_world(self): + detect_range = self.args.range + self.world_ax.cla() + n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size) + ego_path_x = self.ego_ttc_data.data[3 * n_ttc_data : 4 * n_ttc_data] + ego_path_y = self.ego_ttc_data.data[4 * n_ttc_data : 5 * n_ttc_data] + self.world_ax.set_aspect("equal") + self.world_ax.scatter(ego_path_x[0], ego_path_y[0], marker="x", c="red", s=15) + min_x, max_x = min(ego_path_x), max(ego_path_x) + min_y, max_y = min(ego_path_y), max(ego_path_y) + x_dir = 1 if ego_path_x[-1] > ego_path_x[0] else -1 + y_dir = 1 if ego_path_y[-1] > ego_path_y[0] else -1 + min_x = min_x - detect_range if x_dir == 1 else min_x - 20 + max_x = max_x + detect_range if x_dir == -1 else max_x + 20 + min_y = min_y - detect_range if y_dir == 1 else min_y - 20 + max_y = max_y + detect_range if y_dir == -1 else max_y + 20 + self.world_ax.set_xlim(min_x, max_x) + self.world_ax.set_ylim(min_y, max_y) + arrows = [ + (x0, y0, math.atan2(x1 - x0, y1 - y0)) + for (x0, y0, x1, y1) in zip( + ego_path_x[0:-1:5], + ego_path_y[0:-1:5], + ego_path_x[4:-1:5], + ego_path_y[4:-1:5], + ) + ] + for x, y, th in arrows: + self.world_ax.arrow( + x, + y, + math.sin(th) * 0.5, + math.cos(th) * 0.5, + head_width=0.1, + head_length=0.1, + ) + + for npc, color in zip(self.npc_vehicles, cycle(self.color_list)): + x, y, th, w, h = npc.x, npc.y, npc.th, npc.width, npc.height + bbox = np.array( + [ + [-w / 2, -h / 2], + [+w / 2, -h / 2], + [+w / 2, +h / 2], + [-w / 2, +h / 2], + [-w / 2, -h / 2], + ] + ).transpose() + Rth = np.array([[math.cos(th), -math.sin(th)], [math.sin(th), math.cos(th)]]) + bbox_rot = Rth @ bbox + self.world_ax.fill(bbox_rot[0, :] + x, bbox_rot[1, :] + y, color, alpha=0.5) + self.world_ax.plot( + [npc.first_collision_x, npc.last_collision_x], + [npc.first_collision_y, npc.last_collision_y], + c=color, + linewidth=3.0, + ) + if npc.dangerous: + self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linewidth=1.5) + else: + self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linestyle="--") + + self.world_ax.plot(ego_path_x, ego_path_y, c="k", linestyle="--") + + def cleanup(self): + if self.args.save: + kwargs_write = {"fps": self.args.fps, "quantizer": "nq"} + imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write) + + def on_plot_timer(self): + with self.lock: + if (not self.ego_ttc_data) or (not self.object_ttc_data): + return + + if not self.last_sub: + return + + now = time.time() + if (now - self.last_sub) > 1.0: + print("elapsed more than 1sec from last sub, exit/save fig") + self.cleanup() + + self.plot_ttc() + self.plot_world() + self.fig.canvas.flush_events() + + if self.args.save: + image = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype="uint8") + image = image.reshape(self.fig.canvas.get_width_height()[::-1] + (3,)) + self.images.append(image) + + def on_ego_ttc(self, msg): + with self.lock: + if int(msg.data[0]) == self.lane_id: + self.ego_ttc_data = msg + self.last_sub = time.time() + + def parse_npc_vehicles(self): + self.npc_vehicles = [] + n_npc_vehicles = int(self.object_ttc_data.layout.dim[0].size) + npc_data_size = int(self.object_ttc_data.layout.dim[1].size) + for i in range(1, n_npc_vehicles): + data = self.object_ttc_data.data[i * npc_data_size : (i + 1) * npc_data_size] + self.npc_vehicles.append(NPC(data)) + + def on_object_ttc(self, msg): + with self.lock: + if int(msg.data[0]) == self.lane_id: + self.object_ttc_data = msg + self.parse_npc_vehicles() + self.last_sub = time.time() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument( + "--lane_id", + type=int, + required=True, + help="lane_id to analyze", + ) + parser.add_argument( + "--range", + type=float, + default=60, + help="detect range for drawing", + ) + parser.add_argument("-s", "--save", action="store_true", help="flag to save gif") + parser.add_argument("--gif", type=str, default="ttc", help="filename of gif file") + parser.add_argument("--fps", type=float, default=5, help="fps of gif") + args = parser.parse_args() + + rclpy.init() + visualizer = TTCVisualizer(args) + rclpy.spin(visualizer) diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 3c0d7fa0b4f1c..5c5f5eac16b81 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -181,6 +181,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); } void IntersectionModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 31b63413a6437..c3b429680ab91 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -138,6 +138,10 @@ IntersectionModule::IntersectionModule( 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( + "~/debug/intersection/object_ttc", 1); } void IntersectionModule::initializeRTCStatus() @@ -1465,12 +1469,22 @@ bool IntersectionModule::checkCollision( // check collision between target_objects predicted path and ego lane // cut the predicted path at passing_time + tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = util::calcIntersectionPassingTime( - path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx, - time_delay, planner_param_.common.intersection_velocity, + path, planner_data_, lane_id_, associative_ids_, closest_idx, + last_intersection_stop_line_candidate_idx, time_delay, + planner_param_.common.intersection_velocity, planner_param_.collision_detection.minimum_ego_predicted_velocity, planner_param_.collision_detection.use_upstream_velocity, - planner_param_.collision_detection.minimum_upstream_velocity); + planner_param_.collision_detection.minimum_upstream_velocity, &ego_ttc_time_array); + + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + ego_ttc_time_array.stamp = path.header.stamp; + ego_ttc_pub_->publish(ego_ttc_time_array); + } + const double passing_time = time_distance_array.back().first; util::cutPredictPathWithDuration(target_objects, clock_, passing_time); @@ -1552,6 +1566,19 @@ bool IntersectionModule::checkCollision( return (object2collision > margin) || (object2collision < 0); }; // check collision between predicted_path and ego_area + tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + object_ttc_time_array.layout.dim.resize(3); + object_ttc_time_array.layout.dim.at(0).label = "objects"; + object_ttc_time_array.layout.dim.at(0).size = 1; // incremented in the loop, first row is lane_id + object_ttc_time_array.layout.dim.at(1).label = + "[x, y, th, length, width, speed, dangerous, ref_obj_enter_time, ref_obj_exit_time, " + "start_time, start_dist, " + "end_time, end_dist, first_collision_x, first_collision_y, last_collision_x, last_collision_y, " + "prd_x[0], ... pred_x[19], pred_y[0], ... pred_y[19]]"; + object_ttc_time_array.layout.dim.at(1).size = 57; + for (unsigned i = 0; i < object_ttc_time_array.layout.dim.at(1).size; ++i) { + object_ttc_time_array.data.push_back(lane_id_); + } bool collision_detected = false; for (const auto & target_object : target_objects->all_attention_objects) { const auto & object = target_object.object; @@ -1650,6 +1677,24 @@ bool IntersectionModule::checkCollision( break; } } + object_ttc_time_array.layout.dim.at(0).size++; + const auto & pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & shape = object.shape; + object_ttc_time_array.data.insert( + object_ttc_time_array.data.end(), + {pos.x, pos.y, tf2::getYaw(object.kinematics.initial_pose_with_covariance.pose.orientation), + shape.dimensions.x, shape.dimensions.y, + object.kinematics.initial_twist_with_covariance.twist.linear.x, + 1.0 * static_cast(collision_detected), ref_object_enter_time, ref_object_exit_time, + start_time_distance_itr->first, start_time_distance_itr->second, + end_time_distance_itr->first, end_time_distance_itr->second, first_itr->position.x, + first_itr->position.y, last_itr->position.x, last_itr->position.y}); + for (unsigned i = 0; i < 20; i++) { + const auto & pos = + predicted_path.path.at(std::min(i, predicted_path.path.size() - 1)).position; + object_ttc_time_array.data.push_back(pos.x); + object_ttc_time_array.data.push_back(pos.y); + } if (collision_detected) { debug_data_.conflicting_targets.objects.push_back(object); break; @@ -1657,6 +1702,13 @@ bool IntersectionModule::checkCollision( } } + if ( + std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != + planner_param_.debug.ttc.end()) { + object_ttc_time_array.stamp = path.header.stamp; + object_ttc_pub_->publish(object_ttc_time_array); + } + return collision_detected; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 93fec171ec0d2..c794ef6c53aad 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include @@ -150,6 +151,10 @@ class IntersectionModule : public SceneModuleInterface double attention_lane_curvature_calculation_ds; double static_occlusion_with_traffic_light_timeout; } occlusion; + struct Debug + { + std::vector ttc; + } debug; }; enum OcclusionType { @@ -363,6 +368,8 @@ class IntersectionModule : public SceneModuleInterface util::DebugData debug_data_; rclcpp::Publisher::SharedPtr decision_state_pub_; + rclcpp::Publisher::SharedPtr ego_ttc_pub_; + rclcpp::Publisher::SharedPtr object_ttc_pub_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 25eaaf4d8cb9d..c27a2ad218ffb 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1305,10 +1305,12 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const std::set & associative_ids, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, - const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity) + const std::shared_ptr & planner_data, const lanelet::Id lane_id, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) { double dist_sum = 0.0; int assigned_lane_found = false; @@ -1374,7 +1376,26 @@ TimeDistanceArray calcIntersectionPassingTime( time_distance_array.emplace_back(passing_time, dist_sum); } - + debug_ttc_array->layout.dim.resize(3); + debug_ttc_array->layout.dim.at(0).label = "lane_id_@[0][0], ttc_time, ttc_dist, path_x, path_y"; + debug_ttc_array->layout.dim.at(0).size = 5; + debug_ttc_array->layout.dim.at(1).label = "values"; + debug_ttc_array->layout.dim.at(1).size = time_distance_array.size(); + for (unsigned i = 0; i < time_distance_array.size(); ++i) { + debug_ttc_array->data.push_back(lane_id); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(t); + } + for (const auto & [t, d] : time_distance_array) { + debug_ttc_array->data.push_back(d); + } + for (const auto & p : smoothed_reference_path.points) { + debug_ttc_array->data.push_back(p.point.pose.position.x); + } + for (const auto & p : smoothed_reference_path.points) { + debug_ttc_array->data.push_back(p.point.pose.position.y); + } return time_distance_array; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 5faacd4325b06..11aab06ff1d94 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,6 +20,8 @@ #include +#include + #include #include #include @@ -146,10 +148,12 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const std::shared_ptr & planner_data, const std::set & associative_ids, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, - const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, - const bool use_upstream_velocity, const double minimum_upstream_velocity); + const std::shared_ptr & planner_data, const lanelet::Id lane_id, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const double intersection_velocity, const double minimum_ego_velocity, + const bool use_upstream_velocity, const double minimum_upstream_velocity, + tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); double calcDistanceUntilIntersectionLanelet( const lanelet::ConstLanelet & assigned_lanelet, From 9628276d68807fd90a1bfc6d304776eabb7e7cc1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 31 Oct 2023 18:25:53 +0900 Subject: [PATCH 204/206] fix(avoidance): discard envelope polygon if the objects move long distance (#5388) * fix(avoidance): discard envelope polygon if the objects move long distance Signed-off-by: satoshi-ota * Update planning/behavior_path_planner/src/utils/avoidance/utils.cpp Co-authored-by: Kyoichi Sugahara * Update planning/behavior_path_planner/src/utils/avoidance/utils.cpp Co-authored-by: Kyoichi Sugahara --------- Signed-off-by: satoshi-ota Co-authored-by: Kyoichi Sugahara --- .../src/utils/avoidance/utils.cpp | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 54a1c0c649916..2e441907b6d19 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -754,26 +754,41 @@ void fillObjectEnvelopePolygon( return; } - const auto envelope_poly = + const auto one_shot_envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); - if (boost::geometry::within(envelope_poly, same_id_obj->envelope_poly)) { + // If the one_shot_envelope_poly is within the registered envelope, use the registered one + if (boost::geometry::within(one_shot_envelope_poly, same_id_obj->envelope_poly)) { object_data.envelope_poly = same_id_obj->envelope_poly; return; } std::vector unions; - boost::geometry::union_(envelope_poly, same_id_obj->envelope_poly, unions); + boost::geometry::union_(one_shot_envelope_poly, same_id_obj->envelope_poly, unions); + // If union fails, use the current envelope if (unions.empty()) { - object_data.envelope_poly = - createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + object_data.envelope_poly = one_shot_envelope_poly; return; } boost::geometry::correct(unions.front()); - object_data.envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); + const auto multi_step_envelope_poly = createEnvelopePolygon(unions.front(), closest_pose, 0.0); + + const auto object_polygon = tier4_autoware_utils::toPolygon2d(object_data.object); + const auto object_polygon_area = boost::geometry::area(object_polygon); + const auto envelope_polygon_area = boost::geometry::area(multi_step_envelope_poly); + + // keep multi-step envelope polygon. + constexpr double THRESHOLD = 5.0; + if (envelope_polygon_area < object_polygon_area * THRESHOLD) { + object_data.envelope_poly = multi_step_envelope_poly; + return; + } + + // use latest one-shot envelope polygon. + object_data.envelope_poly = one_shot_envelope_poly; } void fillObjectMovingTime( From 19988807d80c5af85ad89b58a723c8891c84e9a8 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Tue, 31 Oct 2023 18:41:23 +0900 Subject: [PATCH 205/206] feat(avoidance): use traffic light signal info (#5395) * feat(utils): add function to calculate shift start/end point Signed-off-by: satoshi-ota * feat(avoidance): add new parameter Signed-off-by: satoshi-ota * feat(avoidance): set shift start/end point with module taking traffic signal into account Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 9 +- .../utils/avoidance/avoidance_module_data.hpp | 15 +- .../utils/avoidance/utils.hpp | 10 ++ .../avoidance/avoidance_module.cpp | 66 ++++++--- .../src/scene_module/avoidance/manager.cpp | 12 +- .../src/utils/avoidance/utils.cpp | 140 ++++++++++++++++++ 6 files changed, 228 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index b0f736916ebda..96b6ca452bbcc 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -191,11 +191,18 @@ # avoidance distance parameters longitudinal: prepare_time: 2.0 # [s] - remain_buffer_distance: 30.0 # [m] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] nominal_avoidance_speed: 8.33 # [m/s] + # return dead line + return_dead_line: + goal: + enable: true # [-] + buffer: 30.0 # [m] + traffic_light: + enable: true # [-] + buffer: 3.0 # [m] # For yield maneuver yield: diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 7976399a7ed40..c8b87c7b4759f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -112,6 +112,14 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; + // consider avoidance return dead line + bool enable_dead_line_for_goal{false}; + bool enable_dead_line_for_traffic_light{false}; + + // module try to return original path to keep this distance from edge point of the path. + double dead_line_buffer_for_goal{0.0}; + double dead_line_buffer_for_traffic_light{0.0}; + // max deceleration for double max_deceleration{0.0}; @@ -217,9 +225,6 @@ struct AvoidanceParameters // nominal avoidance sped double nominal_avoidance_speed{0.0}; - // module try to return original path to keep this distance from edge point of the path. - double remain_buffer_distance{0.0}; - // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. double soft_road_shoulder_margin{1.0}; @@ -517,6 +522,10 @@ struct AvoidancePlanningData bool found_avoidance_path{false}; double to_stop_line{std::numeric_limits::max()}; + + double to_start_point{std::numeric_limits::lowest()}; + + double to_return_point{std::numeric_limits::max()}; }; /* diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index b8b4efb7aed57..2e04935e37336 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -172,6 +172,16 @@ std::pair separateObjectsByPath( DrivableLanes generateExpandDrivableLanes( const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, const std::shared_ptr & parameters); + +double calcDistanceToReturnDeadLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + +double calcDistanceToAvoidStartLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 5423cff11bfeb..9e4a3a928c4ca 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -288,6 +288,12 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat data.reference_path, 0, data.reference_path.points.size(), calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); + data.to_return_point = utils::avoidance::calcDistanceToReturnDeadLine( + data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); + + data.to_start_point = utils::avoidance::calcDistanceToAvoidStartLine( + data.current_lanelets, data.reference_path_rough, planner_data_, parameters_); + // target objects for avoidance fillAvoidanceTargetObjects(data, debug); @@ -1066,18 +1072,35 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( : 0.0; const auto offset = object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + const auto to_shift_end = o.longitudinal - offset; const auto path_front_to_ego = avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); - al_avoid.start_longitudinal = - std::max(o.longitudinal - offset - feasible_avoid_distance, 1e-3); + // start point (use previous linear shift length as start shift length.) + al_avoid.start_longitudinal = [&]() { + const auto nearest_avoid_distance = std::max(to_shift_end - feasible_avoid_distance, 1e-3); + + if (data.to_start_point > to_shift_end) { + return nearest_avoid_distance; + } + + const auto minimum_avoid_distance = + helper_.getMinAvoidanceDistance(feasible_shift_length.get() - current_ego_shift); + const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3); + + return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); + }(); + al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose; al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); + // end point al_avoid.end_shift_length = feasible_shift_length.get(); - al_avoid.end_longitudinal = o.longitudinal - offset; + al_avoid.end_longitudinal = to_shift_end; + + // misc al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; al_avoid.object_on_right = utils::avoidance::isOnRight(o); @@ -1086,18 +1109,24 @@ AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidLine al_return; { const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; - // The end_margin also has the purpose of preventing the return path from NOT being - // triggered at the end point. - const auto return_remaining_distance = std::max( - data.arclength_from_ego.back() - o.longitudinal - offset - - parameters_->remain_buffer_distance, - 0.0); + const auto to_shift_start = o.longitudinal + offset; + // start point al_return.start_shift_length = feasible_shift_length.get(); + al_return.start_longitudinal = to_shift_start; + + // end point + al_return.end_longitudinal = [&]() { + if (data.to_return_point > to_shift_start) { + return std::clamp( + data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start); + } + + return to_shift_start + feasible_return_distance; + }(); al_return.end_shift_length = 0.0; - al_return.start_longitudinal = o.longitudinal + offset; - al_return.end_longitudinal = - o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); + + // misc al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; al_return.object_on_right = utils::avoidance::isOnRight(o); @@ -1795,7 +1824,9 @@ AvoidLineArray AvoidanceModule::addReturnShiftLine( return ret; } - const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; + const auto remaining_distance = std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, + avoid_data_.to_return_point); // If the avoidance point has already been set, the return shift must be set after the point. const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); @@ -2859,8 +2890,8 @@ void AvoidanceModule::insertReturnDeadLine( { const auto & data = avoid_data_; - if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) { - RCLCPP_DEBUG(getLogger(), "goal is far enough."); + if (data.to_return_point > planner_data_->parameters.forward_path_length) { + RCLCPP_DEBUG(getLogger(), "return dead line is far enough."); return; } @@ -2872,10 +2903,7 @@ void AvoidanceModule::insertReturnDeadLine( } const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); - - const auto to_goal = calcSignedArcLength( - shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); - const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + const auto to_stop_line = data.to_return_point - min_return_distance; // If we don't need to consider deceleration constraints, insert a deceleration point // and return immediately diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 33c6982f99fd9..6ed67fc817f28 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -223,13 +223,23 @@ AvoidanceModuleManager::AvoidanceModuleManager( std::string ns = "avoidance.avoidance.longitudinal."; p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); - p.remain_buffer_distance = getOrDeclareParameter(*node, ns + "remain_buffer_distance"); p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); p.nominal_avoidance_speed = getOrDeclareParameter(*node, ns + "nominal_avoidance_speed"); } + // avoidance maneuver (return shift dead line) + { + std::string ns = "avoidance.avoidance.return_dead_line."; + p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); + p.enable_dead_line_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.enable"); + p.dead_line_buffer_for_goal = getOrDeclareParameter(*node, ns + "goal.buffer"); + p.dead_line_buffer_for_traffic_light = + getOrDeclareParameter(*node, ns + "traffic_light.buffer"); + } + // yield { std::string ns = "avoidance.yield."; diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 2e441907b6d19..f1bbb3bc28a36 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -48,6 +48,7 @@ namespace behavior_path_planner::utils::avoidance { +using autoware_perception_msgs::msg::TrafficSignalElement; using motion_utils::calcLongitudinalOffsetPoint; using motion_utils::calcSignedArcLength; using motion_utils::findNearestIndex; @@ -225,6 +226,86 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) { base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); } + +bool hasTrafficLightCircleColor(const TrafficSignal & tl_state, const uint8_t & lamp_color) +{ + const auto it_lamp = + std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { + return x.shape == TrafficSignalElement::CIRCLE && x.color == lamp_color; + }); + + return it_lamp != tl_state.elements.end(); +} + +bool hasTrafficLightShape(const TrafficSignal & tl_state, const uint8_t & lamp_shape) +{ + const auto it_lamp = std::find_if( + tl_state.elements.begin(), tl_state.elements.end(), + [&lamp_shape](const auto & x) { return x.shape == lamp_shape; }); + + return it_lamp != tl_state.elements.end(); +} + +bool isTrafficSignalStop(const lanelet::ConstLanelet & lanelet, const TrafficSignal & tl_state) +{ + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::GREEN)) { + return false; + } + + if (hasTrafficLightCircleColor(tl_state, TrafficSignalElement::UNKNOWN)) { + return false; + } + + const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); + + if (turn_direction == "else") { + return true; + } + if ( + turn_direction == "right" && + hasTrafficLightShape(tl_state, TrafficSignalElement::RIGHT_ARROW)) { + return false; + } + if ( + turn_direction == "left" && hasTrafficLightShape(tl_state, TrafficSignalElement::LEFT_ARROW)) { + return false; + } + if ( + turn_direction == "straight" && + hasTrafficLightShape(tl_state, TrafficSignalElement::UP_ARROW)) { + return false; + } + + return true; +} + +std::optional calcDistanceToRedTrafficLight( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data) +{ + for (const auto & lanelet : lanelets) { + for (const auto & element : lanelet.regulatoryElementsAs()) { + const auto traffic_signal_stamped = planner_data->getTrafficSignal(element->id()); + if (!traffic_signal_stamped.has_value()) { + continue; + } + + if (!isTrafficSignalStop(lanelet, traffic_signal_stamped.value().signal)) { + continue; + } + + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + lanelet::ConstLineString3d stop_line = *(element->stopLine()); + const auto x = 0.5 * (stop_line.front().x() + stop_line.back().x()); + const auto y = 0.5 * (stop_line.front().y() + stop_line.back().y()); + const auto z = 0.5 * (stop_line.front().z() + stop_line.back().z()); + + return calcSignedArcLength(path.points, ego_pos, tier4_autoware_utils::createPoint(x, y, z)); + } + } + + return std::nullopt; +} } // namespace bool isOnRight(const ObjectData & obj) @@ -1902,4 +1983,63 @@ DrivableLanes generateExpandDrivableLanes( return current_drivable_lanes; } + +double calcDistanceToAvoidStartLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (lanelets.empty()) { + return std::numeric_limits::lowest(); + } + + double distance_to_return_dead_line = std::numeric_limits::lowest(); + + // dead line stop factor(traffic light) + if (parameters->enable_dead_line_for_traffic_light) { + const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data); + if (to_traffic_light.has_value()) { + distance_to_return_dead_line = std::max( + distance_to_return_dead_line, + to_traffic_light.value() + parameters->dead_line_buffer_for_traffic_light); + } + } + + return distance_to_return_dead_line; +} + +double calcDistanceToReturnDeadLine( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (lanelets.empty()) { + return std::numeric_limits::max(); + } + + double distance_to_return_dead_line = std::numeric_limits::max(); + + // dead line stop factor(traffic light) + if (parameters->enable_dead_line_for_traffic_light) { + const auto to_traffic_light = calcDistanceToRedTrafficLight(lanelets, path, planner_data); + if (to_traffic_light.has_value()) { + distance_to_return_dead_line = std::min( + distance_to_return_dead_line, + to_traffic_light.value() - parameters->dead_line_buffer_for_traffic_light); + } + } + + // dead line for goal + if (parameters->enable_dead_line_for_goal) { + if (planner_data->route_handler->isInGoalRouteSection(lanelets.back())) { + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + const auto to_goal_distance = + calcSignedArcLength(path.points, ego_pos, path.points.size() - 1); + distance_to_return_dead_line = std::min( + distance_to_return_dead_line, to_goal_distance - parameters->dead_line_buffer_for_goal); + } + } + + return distance_to_return_dead_line; +} } // namespace behavior_path_planner::utils::avoidance From a2b114da5f16d097b45128e29a4f94a325967030 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Tue, 31 Oct 2023 23:12:42 +0900 Subject: [PATCH 206/206] feat(map_based_prediciton): use orientation reliability in prediction (#5435) * replace object yaw with lanelets yaw when object orientation is unavailable Signed-off-by: yoshiri * fix unintended behavior: add escape and move replace state before path generation Signed-off-by: yoshiri * Update perception/map_based_prediction/src/map_based_prediction_node.cpp Co-authored-by: Kyoichi Sugahara --------- Signed-off-by: yoshiri Co-authored-by: Kyoichi Sugahara --- .../src/map_based_prediction_node.cpp | 38 ++++++++++++++++++- 1 file changed, 36 insertions(+), 2 deletions(-) diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 08fc06330b1d8..2ceb22fd6e7b9 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -687,6 +687,32 @@ lanelet::Lanelets getLeftOppositeLanelets( return opposite_lanelets; } + +void replaceObjectYawWithLaneletsYaw( + const LaneletsData & current_lanelets, TrackedObject & transformed_object) +{ + // return if no lanelet is found + if (current_lanelets.empty()) return; + auto & pose_with_cov = transformed_object.kinematics.pose_with_covariance; + // for each lanelet, calc lanelet angle and calculate mean angle + double sum_x = 0.0; + double sum_y = 0.0; + for (const auto & current_lanelet : current_lanelets) { + const auto lanelet_angle = + lanelet::utils::getLaneletAngle(current_lanelet.lanelet, pose_with_cov.pose.position); + sum_x += std::cos(lanelet_angle); + sum_y += std::sin(lanelet_angle); + } + const double mean_yaw_angle = std::atan2(sum_y, sum_x); + double roll, pitch, yaw; + tf2::Quaternion original_quaternion; + tf2::fromMsg(pose_with_cov.pose.orientation, original_quaternion); + tf2::Matrix3x3(original_quaternion).getRPY(roll, pitch, yaw); + tf2::Quaternion filtered_quaternion; + filtered_quaternion.setRPY(roll, pitch, mean_yaw_angle); + pose_with_cov.pose.orientation = tf2::toMsg(filtered_quaternion); +} + } // namespace MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) @@ -929,11 +955,19 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt getDebugMarker(object, max_prob_path->maneuver, debug_markers.markers.size()); debug_markers.markers.push_back(debug_marker); + // Fix object angle if its orientation unreliable (e.g. far object by radar sensor) + // This prevent bending predicted path + TrackedObject yaw_fixed_transformed_object = transformed_object; + if ( + transformed_object.kinematics.orientation_availability == + autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE) { + replaceObjectYawWithLaneletsYaw(current_lanelets, yaw_fixed_transformed_object); + } // Generate Predicted Path std::vector predicted_paths; for (const auto & ref_path : ref_paths) { - PredictedPath predicted_path = - path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle( + yaw_fixed_transformed_object, ref_path.path); if (predicted_path.path.empty()) { continue; }