From 92bec0dc0832fd66b77b5bffac19dc3b9685e3fb Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 30 Sep 2024 09:35:05 +0900 Subject: [PATCH 01/27] fix(autoware_map_based_prediction): adjust lateral duration when object is behind reference path (#8973) fix: adjust lateral duration when object is behind reference path Signed-off-by: Taekjin LEE --- .../src/path_generator.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/perception/autoware_map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp index 210795d7e6b25..a2a1b8b3d3fda 100644 --- a/perception/autoware_map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -250,6 +250,15 @@ PredictedPath PathGenerator::generatePolynomialPath( terminal_point.d_vel = 0.0; terminal_point.d_acc = 0.0; + // if the object is behind of the reference path adjust the lateral_duration to reach the start of + // the reference path + double lateral_duration_adjusted = lateral_duration; + if (current_point.s < 0.0) { + const double distance_to_start = -current_point.s; + const double duration_to_reach = distance_to_start / terminal_point.s_vel; + lateral_duration_adjusted = std::max(lateral_duration, duration_to_reach); + } + // calculate terminal d position, based on backlash width { if (backlash_width < 0.01 /*m*/) { @@ -259,7 +268,7 @@ PredictedPath PathGenerator::generatePolynomialPath( } else { const double return_width = path_width / 2.0; // [m] const double current_momentum_d = - current_point.d + 0.5 * current_point.d_vel * lateral_duration; + current_point.d + 0.5 * current_point.d_vel * lateral_duration_adjusted; const double momentum_d_abs = std::abs(current_momentum_d); if (momentum_d_abs < backlash_width) { @@ -282,8 +291,8 @@ PredictedPath PathGenerator::generatePolynomialPath( } // Step 2. Generate Predicted Path on a Frenet coordinate - const auto frenet_predicted_path = - generateFrenetPath(current_point, terminal_point, ref_path_len, duration, lateral_duration); + const auto frenet_predicted_path = generateFrenetPath( + current_point, terminal_point, ref_path_len, duration, lateral_duration_adjusted); // Step 3. Interpolate Reference Path for converting predicted path coordinate const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); From 841c331f0f13b475d5a2f4a3934429818297c18c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Mon, 30 Sep 2024 10:01:48 +0900 Subject: [PATCH 02/27] fix(goal_planner): fix freespace planning chattering (#8981) Signed-off-by: kosuke55 --- .../src/goal_planner_module.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index ec7a8e12de573..4a59857725a5e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -1329,9 +1329,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( return getPreviousModuleOutput(); } + const bool is_freespace = + thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && + !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, @@ -1370,9 +1373,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( setOutput(context_data, output); // return to lane parking if it is possible - if ( - thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE && - canReturnToLaneParking(context_data)) { + if (is_freespace && canReturnToLaneParking(context_data)) { thread_safe_data_.set_pull_over_path(thread_safe_data_.get_lane_parking_pull_over_path()); } From 706fd9708cf3a0edf6c1d61e0a7fd4f9dc614459 Mon Sep 17 00:00:00 2001 From: Ryuta Kambe Date: Mon, 30 Sep 2024 10:55:31 +0900 Subject: [PATCH 03/27] ci(cppcheck): add unusedFunction suppression (#8978) Signed-off-by: Ryuta Kambe --- .cppcheck_suppressions | 1 + 1 file changed, 1 insertion(+) diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index f95f6f238491b..5e1035de20c64 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -4,5 +4,6 @@ checkersReport missingInclude missingIncludeSystem unmatchedSuppression +unusedFunction useInitializationList useStlAlgorithm From 1442d1882aaff6d84d1c947f3c854678633ba5c5 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Mon, 30 Sep 2024 10:59:53 +0900 Subject: [PATCH 04/27] fix(autoware_lidar_centerpoint): convert object's velocity to follow its definition (#8980) * fix: convert object's velocity to follow its definition in box3DToDetectedObject function Signed-off-by: Taekjin LEE * Update perception/autoware_lidar_centerpoint/lib/ros_utils.cpp Co-authored-by: Kenzo Lobos Tsunekawa --------- Signed-off-by: Taekjin LEE Co-authored-by: Kenzo Lobos Tsunekawa --- perception/autoware_lidar_centerpoint/lib/ros_utils.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index d7fc0aef204be..fc383a8d1da00 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -68,8 +68,10 @@ void box3DToDetectedObject( float vel_x = box3d.vel_x; float vel_y = box3d.vel_y; geometry_msgs::msg::Twist twist; - twist.linear.x = std::sqrt(std::pow(vel_x, 2) + std::pow(vel_y, 2)); - twist.angular.z = 2 * (std::atan2(vel_y, vel_x) - yaw); + twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y; + twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; + twist.angular.z = 0; // angular velocity is not supported + obj.kinematics.twist_with_covariance.twist = twist; obj.kinematics.has_twist = has_twist; if (has_variance) { From 35e9f4461b6d40058e111e2e187fed7793e8116b Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Mon, 30 Sep 2024 11:24:27 +0900 Subject: [PATCH 05/27] fix(interpolation): fix bug of interpolation (#8969) fix bug of interpolation Signed-off-by: Y.Hisaki --- .../src/scene_intersection_collision.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index ec6610048dc79..54af88c2f0fbb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -692,10 +692,11 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( object_info->predicted_object()); continue; } - if (!object_info->unsafe_info()) { + const auto unsafe_info_opt = object_info->unsafe_info(); + if (!unsafe_info_opt) { continue; } - const auto & unsafe_info = object_info->unsafe_info().value(); + const auto & unsafe_info = unsafe_info_opt.value(); // ========================================================================================== // if ego is over the pass judge lines, then the visualization as "too_late_objects" or // "misjudge_objects" is more important than that for "unsafe" From 72afef1df5795d00c1fd5428a3f9001d4178863a Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 30 Sep 2024 11:53:29 +0900 Subject: [PATCH 06/27] chore(codecov): ignore filename with 'debug' basename (#8984) Signed-off-by: Mamoru Sobue --- codecov.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/codecov.yaml b/codecov.yaml index 255312a29ccf7..c907c24818735 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -30,3 +30,4 @@ flag_management: ignore: - "**/test/*" - "**/test/**/*" + - "**/debug.*" From e9468b702ad7a5a0ab9b703b32746b1e1efdb303 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Mon, 30 Sep 2024 15:47:33 +0900 Subject: [PATCH 07/27] refactor(bpp): simplify ExtendedPredictedObject and add new member variables (#8889) * simplify ExtendedPredictedObject and add new member variables Signed-off-by: Zulfaqar Azmi * replace self polygon to initial polygon Signed-off-by: Zulfaqar Azmi * comment Signed-off-by: Zulfaqar Azmi * add comments to dist of ego Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 22 +++++------ .../src/utils/utils.cpp | 19 +++++----- .../path_safety_checker_parameters.hpp | 37 ++++++++++++------- .../src/marker_utils/utils.cpp | 2 +- .../path_safety_checker/objects_filtering.cpp | 2 +- .../path_safety_checker/safety_check.cpp | 9 ++--- .../src/start_planner_module.cpp | 2 +- .../src/scene.cpp | 10 ++--- 8 files changed, 56 insertions(+), 47 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 4bde13165db0a..a91aaba86bb1a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -483,14 +483,14 @@ void NormalLaneChange::insertStopPoint( for (const auto & object : target_objects.current_lane) { // check if stationary - const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + const auto obj_v = std::abs(object.initial_twist.linear.x); if (obj_v > lane_change_parameters_->stopped_object_velocity_threshold) { continue; } // calculate distance from path front to the stationary object polygon on the ego lane. const auto polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape).outer(); for (const auto & polygon_p : polygon) { const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); const auto lateral_fp = autoware::motion_utils::calcLateralOffset(path.points, p_fp); @@ -535,21 +535,21 @@ void NormalLaneChange::insertStopPoint( const bool has_blocking_target_lane_obj = std::any_of( target_objects.target_lane_leading.begin(), target_objects.target_lane_leading.end(), [&](const auto & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); + const auto v = std::abs(o.initial_twist.linear.x); if (v > lane_change_parameters_->stopped_object_velocity_threshold) { return false; } // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( - autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + autoware::universe_utils::toPolygon2d(o.initial_pose, o.shape).outer(), lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; } - const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose); return stopping_distance_for_obj < distance_to_target_lane_obj && distance_to_target_lane_obj < distance_to_ego_lane_obj; }); @@ -2121,10 +2121,10 @@ bool NormalLaneChange::has_collision_with_decel_patterns( utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = (obj.initial_twist.twist.linear.x <= - lane_change_parameters_->stopped_object_velocity_threshold) - ? lane_change_parameters_->rss_params_for_parked - : rss_param; + const auto selected_rss_param = + (obj.initial_twist.linear.x <= lane_change_parameters_->stopped_object_velocity_threshold) + ? lane_change_parameters_->rss_params_for_parked + : rss_param; return is_collided( lane_change_path.path, obj, ego_predicted_path, selected_rss_param, debug_data); }); @@ -2218,10 +2218,10 @@ bool NormalLaneChange::isVehicleStuck( const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length; for (const auto & object : lane_change_debug_.filtered_objects.current_lane) { - const auto & p = object.initial_pose.pose; // TODO(Horibe): consider footprint point + const auto & p = object.initial_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 + if (std::abs(object.initial_twist.linear.x) > 0.3) { // check if stationary continue; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index d86cb0e224625..8794d79a4d10f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -711,12 +711,12 @@ bool isParkedObject( using lanelet::geometry::toArcCoordinates; const double object_vel_norm = - std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y); + std::hypot(object.initial_twist.linear.x, object.initial_twist.linear.y); if (object_vel_norm > static_object_velocity_threshold) { return false; } - const auto & object_pose = object.initial_pose.pose; + const auto & object_pose = object.initial_pose; const auto object_closest_index = autoware::motion_utils::findNearestIndex(path.points, object_pose.position); const auto object_closest_pose = path.points.at(object_closest_index).point.pose; @@ -783,7 +783,7 @@ bool isParkedObject( { using lanelet::geometry::distance2d; - const auto & obj_pose = object.initial_pose.pose; + const auto & obj_pose = object.initial_pose; const auto & obj_shape = object.shape; const auto obj_poly = autoware::universe_utils::toPolygon2d(obj_pose, obj_shape); const auto obj_point = obj_pose.position; @@ -842,7 +842,7 @@ bool passed_parked_objects( const auto & leading_obj = objects.at(*leading_obj_idx); auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = - autoware::universe_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); + autoware::universe_utils::toPolygon2d(leading_obj.initial_pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { return true; } @@ -874,7 +874,7 @@ bool passed_parked_objects( const auto current_pose = common_data_ptr->get_ego_pose(); const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( - current_lane_path.points, current_pose.position, leading_obj.initial_pose.pose.position); + current_lane_path.points, current_pose.position, leading_obj.initial_pose.position); if (dist_ego_to_obj < lane_change_path.info.length.lane_changing) { return true; @@ -903,12 +903,11 @@ std::optional getLeadingStaticObjectIdx( std::optional leading_obj_idx = std::nullopt; for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { const auto & obj = objects.at(obj_idx); - const auto & obj_pose = obj.initial_pose.pose; + const auto & obj_pose = obj.initial_pose; // ignore non-static object // TODO(shimizu): parametrize threshold - const double obj_vel_norm = - std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y); + const double obj_vel_norm = std::hypot(obj.initial_twist.linear.x, obj.initial_twist.linear.y); if (obj_vel_norm > 1.0) { continue; } @@ -964,8 +963,8 @@ ExtendedPredictedObject transform( const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.stopped_object_velocity_threshold; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; - const double obj_vel_norm = std::hypot( - extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y); + const double obj_vel_norm = + std::hypot(extended_object.initial_twist.linear.x, extended_object.initial_twist.linear.y); extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 0e7d1cee65f02..795dc6d145190 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #include +#include #include #include @@ -23,7 +24,7 @@ #include -#include +#include #include #include #include @@ -33,7 +34,9 @@ namespace autoware::behavior_path_planner::utils::path_safety_checker { using autoware::universe_utils::Polygon2d; +using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -60,8 +63,8 @@ struct PoseWithVelocityAndPolygonStamped : public PoseWithVelocityStamped Polygon2d poly; PoseWithVelocityAndPolygonStamped( - const double time, const Pose & pose, const double velocity, const Polygon2d & poly) - : PoseWithVelocityStamped(time, pose, velocity), poly(poly) + const double time, const Pose & pose, const double velocity, Polygon2d poly) + : PoseWithVelocityStamped(time, pose, velocity), poly(std::move(poly)) { } }; @@ -75,22 +78,30 @@ struct PredictedPathWithPolygon struct ExtendedPredictedObject { unique_identifier_msgs::msg::UUID uuid; - geometry_msgs::msg::PoseWithCovariance initial_pose; - geometry_msgs::msg::TwistWithCovariance initial_twist; - geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_perception_msgs::msg::Shape shape; - std::vector classification; + Pose initial_pose; + Twist initial_twist; + Shape shape; + ObjectClassification classification; + Polygon2d initial_polygon; std::vector predicted_paths; + double dist_from_ego{0.0}; ///< Distance from ego to obj, can be arc length or euclidean. ExtendedPredictedObject() = default; explicit ExtendedPredictedObject(const PredictedObject & object) : uuid(object.object_id), - initial_pose(object.kinematics.initial_pose_with_covariance), - initial_twist(object.kinematics.initial_twist_with_covariance), - initial_acceleration(object.kinematics.initial_acceleration_with_covariance), - shape(object.shape), - classification(object.classification) + initial_pose(object.kinematics.initial_pose_with_covariance.pose), + initial_twist(object.kinematics.initial_twist_with_covariance.twist), + shape(object.shape) { + classification.label = std::invoke([&]() { + auto max_elem = std::max_element( + object.classification.begin(), object.classification.end(), + [](const auto & a, const auto & b) { return a.probability < b.probability; }); + + return (max_elem != object.classification.end()) ? max_elem->label + : ObjectClassification::UNKNOWN; + }); + initial_polygon = autoware::universe_utils::toPolygon2d(initial_pose, shape); } }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index d57c2339041f4..90693aa65e661 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -608,7 +608,7 @@ MarkerArray showFilteredObjects( cube_marker = default_cube_marker(1.0, 1.0, color); cube_marker.pose = pose; }; - insert_cube_marker(obj.initial_pose.pose); + insert_cube_marker(obj.initial_pose); }); return marker_array; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index 1de55dca29347..25b307ab2cc4d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -327,7 +327,7 @@ ExtendedPredictedObject transform( { ExtendedPredictedObject extended_object(object); - const auto obj_velocity = extended_object.initial_twist.twist.linear.x; + const auto obj_velocity = extended_object.initial_twist.linear.x; extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size()); for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index e2ff8ce7b5195..59fc726bd10a0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -593,7 +593,7 @@ std::vector getCollidedPolygons( { debug.ego_predicted_path = predicted_ego_path; debug.obj_predicted_path = target_object_path.path; - debug.current_obj_pose = target_object.initial_pose.pose; + debug.current_obj_pose = target_object.initial_pose; } std::vector collided_polygons{}; @@ -709,11 +709,10 @@ bool checkPolygonsIntersects( CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) { CollisionCheckDebug debug; - debug.current_obj_pose = obj.initial_pose.pose; - debug.extended_obj_polygon = - autoware::universe_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + debug.current_obj_pose = obj.initial_pose; + debug.extended_obj_polygon = autoware::universe_utils::toPolygon2d(obj.initial_pose, obj.shape); debug.obj_shape = obj.shape; - debug.current_twist = obj.initial_twist.twist; + debug.current_twist = obj.initial_twist; return {autoware::universe_utils::toBoostUUID(obj.uuid), debug}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 4b3a76c8530dd..9df0791ed9375 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -559,7 +559,7 @@ bool StartPlannerModule::isPreventingRearVehicleFromPassingThrough(const Pose & target_objects_on_lane.on_current_lane.begin(), target_objects_on_lane.on_current_lane.end(), [&](const auto & o) { const auto arc_length = autoware::motion_utils::calcSignedArcLength( - centerline_path.points, ego_pose.position, o.initial_pose.pose.position); + centerline_path.points, ego_pose.position, o.initial_pose.position); if (arc_length > 0.0) return; if (std::abs(arc_length) >= std::abs(arc_length_to_closet_object)) return; arc_length_to_closet_object = arc_length; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index e3ecdff3759c0..4d13d4164d263 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -838,20 +838,20 @@ bool StaticObstacleAvoidanceModule::isSafePath( auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = - autoware::universe_utils::toPolygon2d(object.initial_pose.pose, object.shape); + autoware::universe_utils::toPolygon2d(object.initial_pose, object.shape); const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); - const auto & object_twist = object.initial_twist.twist; + const auto & object_twist = object.initial_twist; const auto v_norm = std::hypot(object_twist.linear.x, object_twist.linear.y); - const auto object_type = utils::getHighestProbLabel(object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto object_type = object.classification; + const auto object_parameter = parameters_->object_parameters.at(object_type.label); const auto is_object_moving = v_norm > object_parameter.moving_speed_threshold; const auto is_object_oncoming = is_object_moving && - utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose); + utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); From 317df4dcad451dc8c577e87683f97783b6375af4 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Mon, 30 Sep 2024 16:37:09 +0900 Subject: [PATCH 08/27] perf(out_of_lane): use intersection with other lanes instead of difference with ego lane (#8870) Signed-off-by: Maxime CLEMENT --- .../README.md | 12 +- .../docs/ego_lane.png | Bin 17530 -> 0 bytes .../docs/other_lanes.png | Bin 0 -> 21164 bytes .../src/calculate_slowdown_points.cpp | 5 +- .../src/debug.cpp | 105 ++++++++++------ .../src/footprint.cpp | 3 + .../src/lanelets_selection.cpp | 116 +++++++++++------- .../src/lanelets_selection.hpp | 12 +- .../src/out_of_lane_collisions.cpp | 56 ++++++--- .../src/out_of_lane_collisions.hpp | 2 +- .../src/out_of_lane_module.cpp | 17 +-- .../src/types.hpp | 10 +- 12 files changed, 216 insertions(+), 122 deletions(-) delete mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png create mode 100644 planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md index 0b6aa697fcbef..dfc88ef676838 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/README.md @@ -27,18 +27,18 @@ The length of the trajectory used for generating the footprints is limited by th ![ego_footprints](./docs/footprints.png) -### 2. Ego lanelets +### 2. Other lanelets -In the second step, we calculate the lanelets followed by the ego trajectory. -We select all lanelets crossed by the trajectory linestring (sequence of trajectory points), as well as their preceding lanelets. +In the second step, we calculate the lanelets where collisions should be avoided. +We consider all lanelets around the ego vehicle that are not crossed by the trajectory linestring (sequence of trajectory points) or their preceding lanelets. -![ego_lane](./docs/ego_lane.png) +![other_lanes](./docs/other_lanes.png) -In the debug visualization the combination of all ego lanelets is shown as a blue polygon. +In the debug visualization, these other lanelets are shown as blue polygons. ### 3. Out of lane areas -Next, for each trajectory point, we create the corresponding out of lane areas by subtracting the ego lanelets (from step 2) from the trajectory point footprint (from step 1). +Next, for each trajectory point, we create the corresponding out of lane areas by intersection the other lanelets (from step 2) with the trajectory point footprint (from step 1). Each area is associated with the lanelets overlapped by the area and with the corresponding ego trajectory point. ![out_of_lane_areas](./docs/out_of_lane_areas.png) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/ego_lane.png deleted file mode 100644 index 65cb73226465ae3de6c27b35562d35c57c5906b5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 17530 zcmbt+cRZEh|MyWTp^y_2*($>jiv)`ZhCH&b_`CIsu_%ImkmZE};1`KvZ4+g`T z#Jd9ilKS1n1pK)9Nzk3I#)}d(@2BU>3%1CK>7_U#O+ELl0v+OTNZ=T{? z@hU3ivlg=zml|mQ&9?ol)S@{e_>6CC^fS-PuLN*16d~EGZ#OngtKQ=Hj0MF;hWrj8 z@|}`Jic3-~1--eKS8uei;pR9?!%YX z$G9VQ-WEiBBxj+aa`R|#`3!&m*K6-N_!0!p!J0K3hZ$S-T&?DstxX&*<*qh-rnA3BZ+!QBzXo5 zX41=ssYH@bhT82SoStpWw6F`R-b7b-7P6_A>vR>eVbJ7KK1)|9m}Kn&^exg0$M~5F z4;=l{ja%{dx0BZTxP&D9-U>fTB_}8EVh+HLj~`Y1*v^=3ru#hAu%q{4m1OPg_TI~v zFU>R~3Kq?CwH}76j+pGvlbSG(+PWV~6>L95*461O)F94Q3l2R#9)p)$K_9h>WiN>R z5^1fRYiiFIsp-tIH9dTQ_^9>E<7YI-qSqVRYXLYb@yC&okv+G^zQqKf5JKlyNcru>rIr`-=c$$^MVTDmKVX4}Jqgzx ze}1OP{<3n|?Y1a9D;R|gFV6zIZjxMHUXG38NK!J&C28=G%04hH*v28zF2!`Z+!Wt( zZeVeeskNS50Xt4B>($p49Ua}$((;Y0vgWO>c~b8U>JCRyYhp1~` zCZ9hk9PHuJHp2J(l%`hABW-y8a_q17Hl~{}Rhx+^;e2Uf%@>*-i@mCRCsdD8F~umi~FXrH`VYp^03KCge}b#6~&vZkAd0|_AQc!t(U?y zGyHkL`+l8s;~?(7|5lpo1%A_GwIHCL6v_KnoH(4uoM^e1v`y{+v*92cJV`pbS^6`85Qu#~Y zj^xzz8K$XebL(wsDMDZG&|Lp~w5RmFE;QUtjndJ&y1MNnpDI1ZT(c^Vy}zT}gfD9; z+va+c{13K%Biu4Y=pQC!Pk+H?S6r|j;s4-rjlKd?`DmfGibA0V{r#5zjv{{7_sJqx zU~G`-t6Y~vhFbqdq1rsR7l#vAWK!XLFTh;HX@w2VKI@TOehk?#`Vs!{@6{aG&R60d zTPD`aO&7lrr$nc}Pd3|{z4k6V54UDBoAEiXnFqsY8=aSWlh`qB`?VRN?}qprj4LB` z^T$q4PYaY&$jHg>%D!XlaWle3ys0juIbFUu#X+1WAdZ&m+s@ijl(bEMzH&}2^xXNQ zDyxnk(hfhXmm>(ly-6KP;zz)RTA9mRJ1;(oUDhqxY#FxG^g)WRN8 zCyJbtp5hM$!!@I${%uVs2&^K%5p!ZpO2t~CjTLX23Nv4`yVV@zaU&^FGLx=bBl>t zw>L+SOD^@<1X_@v!6g09-@heeUc=YU>%UptgRQ6uKL2h-(Eo?TU+-ZO^H!dHGhqie zVbHQ=o8!R#e(e0C5r^sH`l3g13Qt*={GUpb{c#qlD!%3aocV9tEufSGPr{x6u`7|6 z1zTxsf=y_tQ7*7Xqh}GG#{6meV@-!y_fGupN0ET7{5vY3I@0;;sg|~`wnPfwJ@eMh zhY|6a(!5pX(&R&ua$^jpUsWiSK9|w>?2XdwER2rkaB!R|ZvV46Otu4x>MCw=xVx%} z%_R|?&0sjKzxPx4<>uv<>NiZ76QkmtR)hxz24Xf(7LEfETFWP8YPPioZ{NP{sl)^J z93gbO#cgfYs^&se(yh>-e1Byq&qHu^(y`5ca{o)=py6qMj|Fd36)m3J(OrC_JQ(fp z=N+HqBfq(mJL=!+)(tC~TYUf8lWKf-qN1Oeoh5IjV%*GgeP=WhKb`NsIb&pKNTn*Y z`rOeMrZMatgadN`P3oJzV8^4;7 zFv-r@`*GJW)&WGf&7F7l7*G9xQ?Z&atCrC+ZNVJCts~B{v0ahZd!5q7am`Cgic92V z#_isO9lAc8b(a_nuUuU^yT=6F*{%%hxyM@yGp=8{%f`I8O=a(d--7V+Z+*Q5i@INE zlU{iA?FNmw&*3NZwC~&KZaVdE2Ulm^*^{nL*4@8Vn&NI_0i^Is+F9+Mm%uir1oH@E zzrNW<+c~4SyfSs)7~gsA^1?=(sNcRVVP$t9!HeC!z(_}LA{ezfsoRr<*STXav(o4SbvR6kVwPJtg(#PVDbDPy<>6K=aN z)T+-44+qxUP_fWh=h0O)R76W+Z*PCIl<w0NY;VlJ6~1_n?A z1G;-&`{elcQbLI4( z@F=DxMO_`8fs~MnH5H?qtZ|*!VXzko=roDKxq$)yi=CV{dD5J~sh}G~f`WpF*!zyn zPTLl0s8*`;`DY|oV43B{mFw&FY!S(Zh#AhO-zMvk6exf8>}&-mqmOIHvM7Qga_C)k zrx!jA9nPrvZ6isCPBu4<+s-RRE7&qsoKIF-f*1<@6?7Gq6#zY}GTupNKv zK^~3o_gKf-a|Hjo{z-bf;{NXH$WJRe6bP8dbuBHeEiKu7BHc#}ep_do7u+c)A|&_Z z>Qb7h)M7l8H`XwfmF4AG;v(qFm1<6D9xtU_tj(UUThbrT9GrXkonJ(7rB>v|h0WWK zZ6BRJ-Z#m>{|LtBl!-qdwazIg9y(|}z@%_poGca>v*Tkhm?K7w5&q5DHm>Y!b?BP| zgVlapJuH>UgrUC?jydvtZnCEMEbr){TWqZj8qF4QgL#TpzisOCQ!Sjx3N7Xd!FJ4v z$JbM+%;@C$hIjqE8{+CL)phhbGmZXI!#s|6{eRyOa%gtnCLHP!&P3(L-6425&~h^I z`77}F0j>JKhz`H~{j2sgTg24c`)nu=2^5%sfPKEfX6q1Z?#DgJMEVfu#`$NWTd#F1 zF@y}dm1;;??b6Z1TN#{2jr<)!A<){7=R9dpPl8(VY3o~BI0aS1_x5!CckUTm#zKPV zNMP~rB}z_d^WX04ix5*7boY+vc?;dqj(rlu$K23+KGWhozq;D3qkDm@vM(cC&@~%* zVKYJ0#SG?td)>P4?BqaogfTZ(@>oG!Xo*6dvF@85NG>}I$K%lwBpD`kQDOFtnk)Wt+t>YI(=VI$(a z&R=SkYFF9NSJlsWDcjv=9rSh!s_0o3o+Zm*exB!~Y?p2~`d6UprioYc#W|W?SI}|h z)CGG@rQyR3y02PmXBkr}F`PTA+d&5Q&Gq#xg0)BUkq3^`UM=d&!^5Qu8f>IO*~JF6 z;V<0lv5SgN#4&9LGtcJ{XIonr8%w$?L+@usO<9wa%rvFj+=o zWT64$RUk+E^2Nh|fBTuw*4!J9dDjDwKKq@ClGxkX?e2Bn6a1>r2l8wQlrfdF_{_Y? zeT6#=pcD|q-gTsYQgtA+xJ0Y*jMyI`EU!+H;=6vq$@9Q_)OL@)zz2%7+$SEa+k+eV z8{riGQ+b-7joQ%oO>louko4u@XnUMV6JL28;i@8YF2&JFCo0>t))JH`Z_<70eR_!1 zi&qN(OXUBX8rIX-L6yemwn@95>hFZb_?=NeHOA&y?7gl%A7Py`7R-B*h0S@g3P&o*E% zIkpB1Gq6D~U#<=1$shT&>T%pruhQ#V-T|Iweq?wVD6M15UUJ3S=?6>w#)`E@$%0KN zWuHqrnDLIK@j1nIJ1zYBr6ZY>!l;PZGpjn>pLA4P?LK>4R$lIV&@|iPx_ZYS)GSzR zHx0t~c=zvU>FJs0?#fV)wF(XhgfNn!7ViU(rEc`M#bQvQ6{+PgD|eLYR)(7Dm5#=z z^f#XzYy!uGmHts(hP*h&lIaE)dCVT?)i*ua8b%o4 zqYJT5E_{G8fMsqnF#t7s+p~#NYw(B@-+nT?q7n}{4_Kr7=7Y@*<<}9fJH3@g_-w~! z$d7As&dSUOm?GQ3LL!P-pwKfnx?j2qZkW|g z)jQanjcs~DBk40%Kn(9R|EQ@22d)rkhVvGxA$O-dcxsvgtVd^z!+}% zjogG1XO_X;^>0BpPR~^fZTz;r-B{0fwU6dU7wVHef6L}6U&4u27=R<*&Bps}f1tk> zunUzKB85Gf%pw*Z@3cb;^;?~n$jr+$Jn6FUrwii_W>QZ?u0F)CS`~!lYdKr1W2Zc^ zW~?(MVh*eCY?8^zbO;%gWsrVHFR{BnI_otW73flT=e}J%Uuy%ImhUZ|7+m0a6^g$l z2UV4qKbovN8s45M8r*vL%I`cr_oIwf`Npp`hKKprvjeB#cS9%`t_37gdWcXwMK~q2@EP@y|yy6qNa&pH}L`U56 zMD+uHRCE~?70A*?G~i}Y;P%H#=eVM-_Vz0zzDG+b^Q)R$RaH&yn<#%&o@GzfW2xFk zv2^FHAR@`R8&?&-9o}wfzWt}rGM^Y^xy7Uh+OLUVBmAC+_d>rtr|K4Q>e8R6`Qh|Y z%is>DvMs;yjh?NkHvb$NLvGxV`WG%PnqN%J%@>aMK%G=kUao3K1_|7)F(v46*>$;ld{ghQ)E7K55V>p8DdnFh#Rr&D+2I_iwp{GA{ua zvbB!}MO7s7&zZZyy)TVz%zN3Ii@BLq26#e))WV~3M_+b7;g4!ZF&w2D<>~FssaaWB zc7ho_7QIW(c|*dYq7iE2uO*y*&oOCkbtBHJJkrBBe;j!h-n^Wc`$T1=Z^^kpLve(E zh8}Lw@s(Yij-c&yTiGuq_a&yH>%=q0r03co9laNDu|0e;GC~eh??3qSeNq`&)EBe) z^!@?NDrc>6&&$`*F}BilY-Xm;{(1Aq8|6Bsl_y$1*92sTc+(84hnzAS8bJLH$3GqY zh4D{lHksR)Z5!My+Xapy23Nn<06Y+p%Z-9Bo#Mt@>Q}5ql*=TJ8Jk=o$3RKALZnA4 zYg-2A#V*3LXU^WSlAy*UCMM=#5#2cN>+9=WZKuu(n|;T}qR5ws zH)0i#>v!{bVCM*3M5`_c8*lGvr}UoWtSoBd_+<2w>Arlnbvo;<^{n45)jqvZ9u8$c z1n^-t-yG%Is;cOwZs!_T?!;(f+=6IVh9$-vF)zvtrXRuoBzQ;E8I(h>TT;J|l6KB2N9|bGnt%cB%$a*HJfLiUVt5WYG&Q z(N;dggNV30npVsuteq4g6gxt$YeDKt`uFc~Rz$W#;YCGcq?B?PIt32KiQ>DnC6 zU`=jl?mnS1B-inm3D@SZDSH?z`joQ$`^phOFKHh!e>fEGe{a?A%5eigL=PEQ_q>GY zrhig_Kww2hLo43xGeqKjxJ74F#wuP;wYYzP3{%Y_^geNP$bFp#u$`|_86VS*jSPK+ z1~v$8u#~fAQh}h9YMFp|oa*{clFjvVFNJJZ22NVPo$$S8rKZOzxvkcY{LAv zm^id``kF$IDb^$y2i%(qK9KoSW3lZ$AbkrM=P$S#Y#1B4w&46}_S5E=nAf*7-s##w zhG;S8?$*F7AaK;!DBZv__kC2cSxS|!S=g8P%hJnf8vU)R@;%6BzS^Y^)dl&Ylfgq9qgT^jSVM`&c z=$;w!JCQY~mdC$Q+Suf6AfiB8lL!SZ?p0B;5b{;>rT8F#HL#5cx4f-U#vktNGY1Ld zSHkqaY#3n|fSH4q|Sb7piP#6%t&=}B7EceBvx!@NXk!Fy2|ZyqU~P#*ssEr zmxbRpBX4Q`QR@n2(|RaDno-z`*9zZ1jJ}18YP^49y#|x;J3R#1U(fTs+s!l7c4V)S zyh9EFZQqYRX{@@D@04A80BioWkP^Fb&;O*Zs`*sQPL(yqyLZ#iNPG}=PA5iu>Ki71 z4J6IyAR%<2KVBpi@*Hn)yTELAVI1ybvFhd8;|@$uU%Q;tIjzP&mASwo9z-n1>>aLe zO^rQwY+<;G}Gs0Fn523(c|JQ#Y{7&!f>21H$&*cQ2ZTl z?L?N6<@(_HKe73n!j<8-r33g^ums3WWQo`52Cva9(TxAalA6C4o8RH!`S=C1B0@0} z@j>h1O(rcalo&Fv$-?q}1FZ z-jBUq#}#)0D!vf1jbG(c8IGL9L*&SR2+v_+wR+FZmI4hnhPv!rgW3clcXwo)jx0r@4fB&pzJ<4AdlCV^3hUFyb+3pyr2=78VtsKs#s1ZKpWU#pyqQ+i)9n2^kDb1kuqlru;dL&O;ilQcReno-tX_t9{_3cOtm)&R1y8JKD|<7Y;V2Jo^xxEWt*V zIi7rlG4sfEl^(dy;g)Ia8w>%*#~T5wc{T`+6a;;)?cRjVSv_Av*ee^h18nyu$xw|o zf_O0cIZ~NN zjrG7!v9bQ*V{k?w6_rZ9$aekFg^3@l&J$%ubhuq3K~SA&b5|{)SWA-Dj74pru)JKG zzR}xVQ=BJn9zHcTMxlWZwO9%#0F1Gst$sKgzHCK6v%MIJi#X{cQ5(#)z?bycdMqv> z47nvU?4RfB3?Y10v_k2rD%3>+Ox`kY(=S(AOs1BWRuSzyVC{RxjG)+`$w;6zX8Y!- zDy{`R>EP2#Si2s?=gp+moY$M-c)q)srth*jBjNW6l$2W{x$BVgnQko0OXW%GE%imq z#oaRJum122s*rPw(Bg2hkJ19?p0f;o<-h0K#tP{X>a#)R>%!1tzGOdj$QSmd{>FXH zeYrJkK6wJEaty}uG>{XF3ft_~DxDVbyk6Mxh4~o5uL>0*5G643GSY^z#9Z0Mg?w?) zV9L=++XYuADfc%e(5v7#faVs^t+0zwg%v?#{hMp5I33UNl^4f(B?_%MM$G*|q$jwVtzVtY0h&<%`FaOG8c6CYG+W}qtUsGzbm#W=M0*h#=a3B~z_jgg>zFZql^#F!R1Ih+q(GaPsmew;*oHS%STq`XcKK%&Ri{r{ zF@*4>IWG^GSJ}MyWNRh}^>7Qtfyc=R%`+EKkZH(d6Z~cYy_?-J@JwBW@4j1XgVz!S zueDMzobW%n0FP&rQ!qccMERh>gZP`n^YW1NX?LK#YIWnf!QfxL1nu zPp3M?buj(cxm)J*FD?~A>KTL)3wQ1Bd*zvSxtJd6(f)0-xkLtoEVVgG>FXY(1USk4 zaklD4n_Ph;dp)50LAxWRDX2{WBj7F_?D52&K&|zDi7^-%_T#UunkMK@8bKfW(x)F*Q*;hGDK+g|X41KO<(o7P-%G^gIh%*Xln9+cFpmzGTX3ee2?fCLL10`A| zH@Lcf3HqU@jgjQfi)er#Vc+e?%``I_wMjFqu0Wq(Tyzm2wmN?$*w#u^*)T)86Hum9 zu&QVFQhG$fR&Sa!)fns}jF!H-jL$0AAW8qZ)OTq7fR(bDp?>SOyBSuv&_Ko_!n>{l_ zw+Zp-(i_xYdDncDDQ&Nzq)?ZsSH=;z68epe@H3+&v*I{ z<)!carQ^ZLgnY~^ze0V%{^~J&O~`5Ttt6-!0-xY4;$mtlYZp=saR-FkG;9>@0&O7n zfiTMqn=OU@Zp>4NJo8m#$+-|XD=4tMdYYz^O=FV!>XJA!(8FEbj-vu`Hokgta8)wV zn5Dk*f^^_ItR-)R0D9m-d;3i=5EuB)IVGwKk0FWgyqi2b=lK?*HXT3QMSLJ$b7tE-!moa{3TE?u~#w|555pT>%1 zPk`HKJru}L)lVh}30(G6hx4GekD3>R(+j6teNUdx9_!SMbHMQ@Cnvd#n_mcK%&P_7 z1lbqNLsdP|eb!fqu%n@&p{uJ4fHoQ!w+C*`bKo8088L3;WMtxlBW~0D{T{caC_+n0 z2KxI2D5F^;9w7Y6Se2^u7ArrswOyPZ?Ia+GQA9xATM-p&{#!Tv=NdPrrly96HCl`> zCNQmqYsXVlhQ!xE4PHHi`-f z9c0X&GW&6=T!FNheY5qXqNJqDj7Q_=nU`;|9wYti!NLB%-6S(TilVK0)V8Zzrt`+_ z=jCMnV|Iet+Izk^VZ8TR{)nE`wRo(j`@Y%&fV5c5&SFo(VFUMT$OqEr0!#F_w3O{x zq-=GhroLBQ#)fa+=R_rHiKER+kU(KBMXV4Hmwi`JVq3E+Ht=DsA^!7gn(3_xg1lGDpUAv65Qi2!3*)$>nO;IZ4T7Vd;DD%%IgtFr%vUs71S;MZ0lMd2#QV12}HbF|XtybrFNP3dYU(K$k3~uJp#q$w@>Bf8f?Hp&73eG#BD*R}dL= zJ;}Sx6I1q28LEoOQ8FKxWemLAvzTu;C>6B3FZu2N8(%`Fgn$3O^ta+=i(Pu5ezDe5 z@mq3`S7l>DJ4@!wPEKZUEtVujFe)Zn_Eh&hj4vqx4rD`O>nUklU@mL+G7pNd;|G6q zSgy|6u=IEd+miD=DPbseQ78@x!5#e$k1X`n<0A|Tv(V=pObKtH>?6&2g(F40MWg;O z$)G%$1Fa4pEv+rIKWGvsxqe$}tTaaK)|Icj&Q!~vyyE&T44V|y69<=Xb$nlPk%3=( z$aM2~LH%G`uKPgE@Y(`Y=q2`A@V2gnKAJrcr^gQrnDvUIiPS}^2xcr* zmaTb}>!2-ou_fWeHFDihAZ5E7YRa5oBS@JRB_$KZqdjtH7HZnJxG1FnxGlfj{;`_< z`u$<~q+IGP=PtptFDy<2=<-G{D?JJ{&v;d97OLOAc||23&!`yhs;UW>)lQoCDdtBS z=;;x2g$zjp-C;}Wu>k8Vv5_ieQgnvP`bZLxjMrriE+>%Za8V~DS&HLy^Y?djq?g!+ zR;;BDa9@~Afe03-QD{RM`oXqjnKzd;qVuD9w~)!532$l0)>^ly$)A_BWzSKe+bvOwReR7Xg7rV)42oeXvuVz%Nrk7WTU&EQ*Dllqh z6GX&W>Z`-)osEPVdg<{3r9TiOK>Gp9ua0rc=(qBT2s?b_A5JeP^Ca~HzAvb-9+$|P z%D`)VkZkak!@BK~60*oHVam0=#zGrx^lPWPEd!3 zKmn(74ES=25V$%;D8M1Q38lu^|8ZR9HgD&)t2HpCkTZn`n|^rq#d%4|M`?tAaEAk( zJCabz;Kx&W^mk8GO$Xx!!W2X}3Xw9QK$)&T8+l&Mh$|I`UZRW+J(T8tc)1fZZuevX zz}n8;_n7gRwYA5{~t&HlSwi6jjat$#d!MM_oKG5Bnf&So;@ax zAp!Y{#zVX-lXaH*uc=uGpHz<0=Yn{OuFho@eS~X2`BZY(7eV+$62mNV!I#j(SV4~( zYi|#<)R&i+cPT6`u_gX|nmiarO-+3lKLj*#7WcTJtXY9=aHpO28C*7=i7heky`}!I z-@oZd8Y*5+*11H5-6kSB`g_P4F>krubJer~7tuw5wyJ5YP>%m;C%C-4?9z}~Xw$kp z8MAOwPK%enAR@rcd1;7VEi>D`+S-8qx9l0SaK4?=Mi@AEc?HZ$3dJ6O}~Q_6NSq8Ql-ub4XFMnQmq z#zVjk#G^2e;5aEyw$yLktT!lUjX2B^AC6B;A#AYFsC$b%8E1{~nDfjQLULl(+_bd= zbs~BGe4wX-GWNdrh8y8yKMRd3Q_z;$+BFTCv$jd(WOeSZJ&=|Zq3h$QAa4EJsP&$p zeU>}#7!}1WR}c5q(hHZIJqLG0us6A-FLxXCMZKi7wFOj5F>6sgvg*048rs^d*AlR( zg=cup!ng_RfIVb0(c6pP_ITv8kEA~!tA>Px7;?J5diANLg&Kh?2W2E|UG9DjK1bVt zA-b`#0Z5`A99X&*;jwu{@!Hfq93u20PFpvrxt*V1tJcF1(~oOj%n84RDr37zOm~Tc zed8E!Ug+akU7r=;$Gg&M`66#$U!6XM{R+dW+0V) z_i6XR<%)~Rte;oQCq^c4v_3K2w%Zo=OO1N+I{?X`sKI;0uc|pZ;NBAO4zgmet^@UW z#-5$##jI2hQ*tJq-knd2(Wilj^NSsexLB#?<7^(XP`WnKkX_qh)on`?Ldb_1%Q2HW z68Ex$&d{pxsD}gAJlg_G8UnW?M}9>zI`F8Da7THyJjh96Wb;Hmp^yL~&DVmx~*pO4c`@?i=! z1CVtEWZzMD|EXd`I}=|cARy?W&6jEJdgSzue8F1G7tv1c9?FoOt)gr(6&em11=($Rasd%W}y=&xjEZfkKBoD`z@Az3$Z}k zXI0a0wX-vbh|Oiq8AMl?y#(5!#GJqe#|PBJ@RVQ@Dd75xcL29m42?z$InI;`*_&xl zgw`6^`$N|(H{pyXCFvb?57w|9UU4}Ybu*=sinVf`NG@YnBi-bsygV;k# zvd!GdSt7F3cM#0TUdELoZZqkxJ|7vpNSFQQ-nx^uSng|T0Tl1+056Qs#KB<$B%5C6 zry?Z2XXWl5v{>4ynVEBxQ0s3`;{uKFT-E^*$nWkF_W`H)X>y-OL zMGbhXQZEwt6qbrmbx2_v|7~obx}_+vNpD;;je)-#w-Y@7HOX@-`iDxb_OSX;U}1rq z@F;O;Q8fiF`x~QKe_NM=6YzQ;d+|hQ?YT*q=<5LUSYw@+YF(kAj@@^vL@rSv?jvas zOsxStWY=)~&kot5!kq{hE>ooVyJ=OVG(lh@(A-Y8MCoO!Me(fWA6{AYIH+CJpSgp` z>dt0V9Jfopz?&KHHe^Dvk|c9`tx)0yxSA*aOV)yS)!C2m9~>OyE$sP=??-gelR(9d zI3n!M^aoQ4rFgjRn|>=kyg;v(jT1kLP;oZCG?nHeLK3W7a0LBZ)8P15QI|R|#UA!_ zCd$v<{h(iH_NSY#jSX80df8c@etyzIEQXZk2lfnC%$dsC$?09pOQ~+BT-N!J_nRk9 zZLBmLKlzPx7C()yalQ=2vRB>Yybj#T>Zt9JOn!-NW{CL5!|6pMjB25g4p#Y!)YS@{A%TUJW7UaEGY)NL0}s+l5kY4PB)`i6$$d|O^c zYoa-{z1V=C{L!pt>N2Sud^(UDtW=S+j{j+UlwOGCK}fD^swY~kCU>65dc%=@6bn)- zjT>tt-A6^=4_P0uG@pR(O&8#KeXItC_NqBQD=fXA8ElxVQSAMM-x%FA3S?hBcfo=| z+>T*N7$fsN(T#qwr-v3d0(N_ss@wmDutoq1Ky|!%lqK&yzgG@Ubg71_gDfMgC5^7D z8o4BLP;$1Kgr?lJE!(KoDeWQ}n+x3_>>$3*5FW-g{o|Ie3F7(y1ZJ#ut;k%*mTAD5 z2HJ5%^$G4!SIW3jJ%90*1i%H4y|BeRiLB>`1|G8N!MxbO!!U+C-2|23(wsv-4Nr_i>cWihV&###T z5#sd1p6bkgE0+c;8evzX6h~3~1qim>^}^1}0`HRh(U6migM&Y#pPR5|+%A`X(QWc8 ze&^AVr+)I$l$NZz$8mO@70D%)d-d)$oXXR7?!}OFMn#|QTGPH1y8XY`gUfZ4lP&3o z`5#5(E~d-=$2;l*n@BM8AV#gL!>KpJ$f(Iff=Fc5L9B3RGSt(Xi&mjrj12%sI8Fkx zEMZoJTsQJqx5XVn0Ut|Zn=XhzsL9ls^O=P@?A{4kCp8w`klNW^LW8nJ@{UdVpHdPf z*}IiNBmkIr>;<@79o%-44*Zm-8v#)~=;}Yq%R#gzouZ-o@)E}PWJzcOqTbSJYj;un z_J9(Cb)d+kdh=nuW3TH*|ME-@&^?u+pGSx~-I2TH3o>V8Q;G;x85xT)!O0o2=VCn* zEgJ+iho=pulUO|EYEJR>N}N21Y%r{xO4<8%qn+C@C4UdB_11$+S&vm(!|%+wcoEbw!jm) z#Q~j_wgm{ZzV9w`n6clvcp_sLcjM+2FIU%mYAP+S$)wMVC#Ia3>?R5ZDlx6#vv28M z0s41B>1Jo5QbL<~B68P9wf!A7Itz+E_665h!Wf%hZAbkU%C0^|1QcRW9MvVh7PG*O@*rkzVr_Dd$i5oevhYYH{LVpH zw?!KEQo>yfuY1&EStD{;<1IZuJ)t|Dc`J_eoZnv1DGiYv#!@aK_yBAfuKZOv!UKPM z!NPwPceQcAdWkinokDpaTCsv7xC(&yk4uhP2Pqb^K-R7ppNL*+oDIV$f~|~EQ;E*~ z_>4O*Vfqs|L04HB^sZz3CNQ?ygB*mvBm#j1?C@Fx;%hrQJ55bZ_ZSlTEfd^XRYbe3 zrm~kCDozfu3qU@uuAi}|B9Qi$RDOKH!`_JGrq{_x($;KQ@joSdAD3_jHn{(dN$PSUi0c@LuL?l6D39tSTfkd`$C z9aRNvbM~Mnt?x3Ua7+wc zjU10!-|=Xvf}k+2!c5qqQQWy!x0v2WFyB&EQu1fVQ+0$woik%ZAUX_G#&2k;O5dFE zU4^MotBwS|UuwuMVTH?vn!aefjf^udZ(9*hQyUqLd_z03Vx zP5=DYJJBzadcrp#x< zoGAyrr}wNq4xo>;Ev`n^QL7Vcy_MYxk;Nsw2T#{X{LkzJ(G&@(`NiqEtkuJwOOjF7 zFGUc~D60+XBL3)Z5SycT!mDQf^v10@;q!T<^F{5Vpm2 z&H4^}^^=VTBV#th7LS06wM?b{I5=aCFg%FL__@>4yx=^tCvJqf$CDdIbU*tGGRoH)3eW3uj%qQ*#_wyf{bwk2oEg6?%GS~PELkA*!;m~|X; z3f;D9BXxIp4nQDFN~DPS*9$j~t{OHFEeE*MK!RwNmK>zuDf}352TO_l!U?j__Go60 zYuU+xiC=hRyY~*&;g&VywzYq~ANUt@;{|eqK-dhIgCM_EU>k7R?B$fhUz6oz2}?eb zvpgtazY=A#VK%n!4Hcy}6IfjJk-84r-4=(+Yc^O#I)qT?6s_b<* z>H=va=*>6?JEo_nb?ZxLh%s)yv+h*#+M`w@w*$W(_$VfaDVPz8p`*xWe1U_8(yZ3? zMsSjeM6o)t{|_*QLK}cUN)ZEWOmze99|RRCK=BUFRG2%iRzG7Uu1enY&PG{ zt_t5)6EL;1F%*D2wF76$OTi2&;K_B2#fooc$;NwdvttsEQV(boN)x+Nt_ zGNu$(HTv`qt!nO1P7dZEVv)B{(v4nwf1&eDbz<*=eR7d7ttBxF^w-Z5Z0g&NK+?_777UOQwcC1F&3Me?i zH4o0dzyVgQ^-xjWg7RML`I`gJZ5Tion+FnN91GApQFjDr%%j3Q=)Q|Z$74GAq>l_U z0Jve>u2ZUAd;3i1rGo>fzFGT_`AToMNLrEjpo@)H(> z0l8TCBX!^4-TTSfl^E3#Lj!}}e5rz4s3Q@=%rJ7RA z9-@Q}ojbnP>FOlcL#K$vAGTL%hJm)GWEvFccfcVC;3`suufl9xTwQ;@;3wGfH-Yccckbv%moQT zZEY=U1UNx^KNI&23;*KB-DLS>_qj}@uQqfwA_;seo3m2 z@)knrpszAwRntRp0vu6io6>P=A>fbvDqZ@$Lc2~)!vY$P8kLw1?q=VU{fh&P|DMHW z!LnzC+vnyli^8`ncfgUVpSHEY3Xw0qeERfNOdJS}jGX)kwB9Zm*}%*Jx-vb%n-*14 z67`}=YdB26;Jnq16}=7IIzSh@+HOR3td&qv=fmQx0Gt8jaXvkJHmwF0w))FTGDnED z_k}?NcfI52p?Hd&;PXn%kXOlZ0JOKA&ENZ7)(zuxz8sB{)lb2Ew8JuCl3WD z06}LE94=wY`AlT)p6g9{IlljnkpB+oYeL<@h+NPe1e9)#Vl4@A5Zo%(T0OARBcG)| z%z4gCO=WVRduV_11IPjUATKEBs-%;gwl@8QPs0;{)$gOC?}H>W$9I3c#(Hp`rF|?5 zuN^d908R(ax@_Op_$LK8-Yy;0`Lk9fCj2`$p`TN^4rqS3N8C{_&`^gn;RSNF9tO7IsqpgP zu?xR;Os5zk zZYkivOzPZjOVhVsAqOqyAkCmSM^EnkEJtSI*U31rRHh=o!tRdXR4#Bbp6}mJ+NifT zfgU^sJ|@-m5?h#q7hR{LCL-Oox4?nHtTgIr7r9mNz?aLO6N6(liJ-|(4F0ZN4^{?C z^_&iT1x%NS)J$Ijrh*GGFRutI|JOb1YjiZsfKkK<7=B>p7QaICRzg6Y$-kgcjS-E> T)*PUKflE>LsZ52mN#OqghXEdX diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/docs/other_lanes.png new file mode 100644 index 0000000000000000000000000000000000000000..a56d6427f59e33723878bfb144665e9b57f84abe GIT binary patch literal 21164 zcmcG$1yEJd*9UqPP(&Ie1Ox#EX-VnsPU(=6R#NgJ-QB4mAl)D!UBV?qq)Vi`8!m4j z|KIm!zIor9d2imlJHrs?oW1v2d+p!)tvH0LC`n^sJjQ^*U|6y;5^6Bm-C*!ViGCOS zB~pAb9ejK6PDaNG2781Lecd?^`iy-FI{T~tEZAhf49jT)bbij2MrPpF9yfPn!gn{HooSm*DUyL zR}?pzU)A#3J-&FbYiXoM&Vo+eG`sh7HX@HSmB=m)zxzJA837jOz+kj!YNTMjFJk(^-(){( z+yggYd|`*dMAWDf@4#SjcMCCLuoyxabMVL)k^hg6n6-c*jZVOz(T#oPGjgKXfuF3}jmgFz| z&_DWEb3XP`AKpCSTlmc}Q+JV{C3%u@PMj|B{`-kb z9oPKKMW4Sz18=gVhUpjQ-%JP4fEcdJh3;cBN4!=Dxi2ol|$-V%lVQaL# z;#(k$GB+^0&)V|g_ITqACp!WY+P@FY(yu?{H~ioURA(4GQuIF9u4avhRE&(ijT6R0 z1KTOW*+Dn;GABGL-OxX*49|6K=ta)$U;YJP18om*g=V}K#mi=l#lJk89V;gDQg5Tn z{(YX0lFU+yH`44$1kVxFyh#ZO>*kTmyHFM?G_R&Sd@XUO5R)yfagbYhvT?AKEyfPz zHGRpU;?7MxJkH|2yL^47DMlqaU4qOaP~w~Fj-&Ky3ABV8Uj=+|91(9G(6yxnV@8}6*A z4(wpKa@RO9HM3kMA|gt&d2>6->c@YdtxJyT@SK}O*Pz*)XRC7qBdB_jxe2>&~cy37|r`7V56hgBqibGv^Ub;`MEmD zymFW=;_7UV0ORj+EdQ}Ng>rOi7gZ`u30tzEd-YCb#xCwJru0xDbFSlX)_Eg?X43C4 zZIXKVH`I;rTcNmDeam>e&rp0#x&k>V2uIsvEVz!Rr@3;3eNM)*XPYcD_gQv_Od_g2 zghbflaKj=nlSY}oOr;!)jwnbtU!6W0cRbnbvCQ`N_=U0V>ORmxA}inwAcsw1^3&lj zweY)_Lz(Il1=@E6dCundu7}{~fBbQ<1wRDTV48NaO=4OH9PEn$Q&rj27b!0_xC(Fm zcDkA_e09~jKv6kmvU5n1c<>g0Q`=9Q*CMSqrw9p&;|=tLp@$D2j*gB7iakx~MPC~6 z*SP09(*EeD&j~qGX0W{>N!Str8i2Q4*$D?GW{S;Oi@c-h>Dt#uH=(pQ@V;!q(`lRX zkwrA{?8niX6&knJ4G1lzQWqL1m@*v}TEv{6^~HCv7`1rq&Ndi7JUkD8))4wrpRXEa zyt=vh=Jxp)XZxWg*4AtGkJ)5Y&jE_Q64gx_a%jC_A3->@Tqv7gDBnwg4=(k7jgB9v zEAOqudIqLY%31w&u~TpK{PGOkRE@OcI3)3c9&SjJUlW3Udj)>;Etg+wF$cfs0^Pv2 zM^DI{oGQ<(Xre!(fFs{Fp`GpOH8r4EDfI;9J`l6bCV8;A#9>v=Z24tP06_Y;rM&4= zm#cd|XUfXiKCQ*JK0CD&F}=9=zzROaui7HpagZrH4~bUoHGm?MSqu(ayOOAWl7h>B z-VRQEuXEiQ=>uudK^J zYHnuZIeT*DXty(6sJ>Te*Aep8#ALB{!tjV@iY)?1_!>7B^XX@;d$3p72CbFfJ~V54 zg(eGHMy!^isThAsru9D$z1 z7%0|WRe~gh&rZ|nAmj7$aIW?AOG*vW)0=n++qbuF_i({>`O@m@D=;(PaCA=U85woY zys5ubzebx7LYttj*3&<47m$#2Qi1yfiw{<@Fkmc9@_%6$5i_s8u;qqd9;eEgJJ{_! z?3npm*`;y)KK6RIY46_fX7Jua-U#rRLaX@O2Zfk1&QrF?j=*Gw#O8*Eg79vv@61n> z1Gh=SR z7Jj}Moay${%qA_{V{?$-O({|CDDRibsYQLjT|QvOzE3+n>KGFR70EaT4Q}mTde!)r z7r(XQiJ48!nVp{e_J}DhxRIwxr^44zPj74gVE`L25Ahe;d<1xxk35~t7Hro(F4Mek zp7O`AFv#YNpkA2fKvdP$*N39V&H~BX-W|!Wc{E$hj8|S+v?uqOY4@<{m%9XiTpmhW zvtVWMQC77-nCO-rJ#J`_L=XG^T_G#iBDN*ipy6N)tsjnQK z#pW~InK8C?O!AtVEnMV?s*ta|rr(-qD0yRMNoUYX)Gk_gXL%(h5D16CL4$4hNJ){7 z<{-Z6XSQ5=NAC2Z2^;7AeqQ{JS6V>!kwrI~z1^fI0_R+xr4Q&cn+$al2ckN0Xx-|n zs>Gb_RGO#qtq3tV^i3J^zNlePeD~ROY2y7d<896Rk#L1*M9<3x0>9i`LpO#;;UTtq z8R?!a8q>dP`c6&^{mqV@u}z=9it*Yu+;WZk{+5B{*GZ##`9xSZ-B?jDu^JrkvY;mq zO=s-i?W8J1y-EMW5z*n+-1E3EqHN)O*UjO#b?X{y+U6~r6aU9(H!NzkvF}BzIz1kmqjI;=~S=l+EjFpQrOITxU-SGGDR{*pf)* zNVc4aG)23Kw!gGO6bXFTe~83b$5JCTPC*SPK@{QQz4?~M=KY)bggGuX5&8&||7PPL zcpkxnr9ZKREo!{9>mRiVkOfS0VWwwAe_-9@{<5mnq3 zCgdoB!p$tKH!&wVXc^*C$Hvm-(oFVu?(<{?LjPr((T5EM)2Oca>^E3Ir=C9h+*gE8qjj+Hw;KR?loCSDFL8l z@!8uP!=h$LTun}1*ld1lv9<0+9~FNO+)R_>L_50ZcyYLbKsLw4k@iLWJ99)#X}Luy zs^oMKrIu=sjUA|POx5h$UX75AGBc{+Y0~yG^}fmMTx;_G`>GGFaQ=mfp+@CDOe^wn74X?z#!T3Q^>cCYe&J%{J? zT^tomwqIVZvPrS$(%1h@xkh^c0uNu+cV^t#LV{U+UqZj?mI}JR;VL(HLfzhH1P*$rxbc1{%?aUyuI@@rjM(6N&1S5uOf@m z%z50&4Sm>><&Ff)68)AT|BS9{WMndxFGVFiH%>+wCG!}cAKs@@@1zxTjatP$K{@;1QS9^P~2Z72p;wt%>JU7tHv@a z%2v&9{sSaV-(BQ!fZr-;(;Vvq40XegW4zLKH?a$E0;~fApR0&D!Xl}l; z-xqHVJc+CCe_d5=D^*%m&K78YG!Q&2lGEpf0&A-{2#yDEK0x1JDz=DKcl-PDk{?j7 zQkkAo<}K=l-&mvi%ju17-y<96KhRv>xeJtr*~}*YIVUcFJclLst411~_4~cBB}u$4 zXz|iAKG-+w(pIkfeRQ>*mLKT2gO#@7A2hBXI1`?mKMq>tc~YpsaG+RmM zoI`^5J5WRIjEoF@!YP1vVjU1~>_AplKhs_`szEN?y;8hvjaRcUmu>HBsgg4R| zEFp1;6s(e_RO9tPdGO`aQXm1{bt?-ADaBugxC!yk6`Kz4-rKgT9MHL&%)8tVbF{JF zr7qq9F_?uVjg0wB%ai9pdQuK+(P(s@l4PoTg|nV_Uka|`639LtQ7z|q^?+|#U4D_V zz>wifw`8OPwXPxeMLsTHD+jl!=EJ4iVh5z9O9NqG)7YQxN)2y-1#}sDhE?6>m5pvQ4Gu4yuduQTTx~ye+s$~hvmMz2Y~nJZ zxivUq?GH0OZl4M2RVCKax}!VbkQ1#f)9ev9ek6Nr?_)fpDf4@Ab3zDY zEF&0<&-lU3{-Tc<|BwS`F9(^Aebk!qXRmWiaudhH^2wh*M>XYvfoQ!Ke{py^!%EP6 zy;&^W;65b+|R|{w%3&l zpZrb-_=-_%t7BWp*;-NYZe5Lp9y;Q8;9Lnzx1nVV+WOaqZ0W5hrL7X&ZWS%ap~FKx zLJF0BU2LK>)bzJHrhE0uX$(xx5`7HvN(s@}sW2yu5X3 zIF6A}iA-x|JOCmB)0 z&zj(T{8t`FOk2hc5y~9@oR6|@q(sbPfss+xaLs?n(NhRBCs9@C7F={kJ71^dV#aE=5ukv zlV0Eg!TeD+Du@zgSh#a$cN7!SEk8zjjgW8GT0G3g&KBb%~k&&R6k3_{sGx(d=M-p3X6>Sf|!iUo-;>q`~QTmua>novyy{A>(3v^Ve;NhP( zXtS`q&!KNe!UNy;7q&=$`^1#0;aLJg))(w`S7?RGspOLZ!1mtrHUy7o!xwY?O|b3OS7! z1__#%%2|J%^ffeecTPO{H4N5gkNk5#Ruc@btf*L6)Sr#6lAs4*Xcz>uY7B!m(}Q1E zKj2W@y5&Vinjg6%oduvxCNy9Ud9#-yv3y^lR#(tj@>RR>cul&K)9vW*-=vy@hZAVW z)hbb6meMpkrGwtb4y>hC}UX8ZP|h zKQJ;fg7wA&sK$*oiSTKdy>8Dle{%Nkcz&E4m7O?A3Unkl*-lTpgb!s6-gF9IlTlwD zM3;X*#K;ty<4(D1c--I0u{1mDx#(1x%!Yl4Fdx;oeeCfBZ_q@+A7KhH>Cit5a#ItB z6dbyZLhe6}Zjf8$1@rD}e0Yz-mDi5S-%dwg6kjzx?$7omVvnP_{Zmy{rAA*;)pFC< zw>77=DLcOFk1#$;0fDp#H6aF?>wdA(4a1?Y)yDNjvfol~+Ue43!;7A1xYx>jGsoGk z0sO|r8hf=xvdU!E%*}6;yzf2y-NDsqIONoWg%bI{m&L{ODnyJn-GOzO-ce^FT3dxa zx-BkJ#SN^N#~Z@%mBj3dCDxYn-NpEu^yQV|eFj1;J*g_6imxwr7sl304a&5Nlg!cd z2sJ6PQ%Td)E{IF)uyINe6otH8P`uTTc|zB}Ct zw-@yKcCv3WSQSC1oFUc8u88176Mg%8%hCE4?aw#+=8+29Cbi-RekyJmzZZ!YgN1Kc zKrkT3H}~fLiD7_8DGIDBc(BEOUZ9H&6XaMyNX~?&rQB`ed4*ShcCs-FQ0Yi{p~)>D zXGZk?RT+&w>A*d-@rj8O|GAEII$N8x)!VEY)v@T%vXQS$vDDPL+Q+xH`N~RFD2fw0 z+2YTW<(Eh$yo=fQbA|H*J`>jq&pKW;Hy4Ujzi3L#z zJ^ct9y2vM2Wu5B>G*LOT421I}eH4{8Tv~b z8p4@`t_Wlrg$k|s!VURk7F-oLs+4In(WH|tEibBZ9~>KlgLM4Mn}UZWWof;4SA~9f zkeLXB0|c+P53{U$ZoXFjs=3+qc7D@~3NXtbPxH)W;e0d`z^7!KEHa;b(0gKVCh{5T zm1?=+97&{i3qCry4<)H)cgvM3N?np4ySyYZgMSy;)wayH{S% z{((bo@k!MKyzSoIN_{XrL97X(c#qxD4ZX=%fZxV!aROgwMQIVP{KWkV=Z+1-Y&GPJ zkWSk`6=s2xmRhH7ra{F*ED7PA?z^Kt_H_O}Kf^W`?tSPUU${|5X=!PvWspd;8#ksp zJ%>sEim7s&;pJ()B{7i&G10YPc9YBZTd#G@Eif7ET685b(Z?2!#|)p^1?l*_XUOf? z4#NHj6T(owohOg-Q+9plOuw;lh=+$uXiU%2QjfCnIUq~u>qF+LsYmrAyompm0$>%Q zE9gASn;h_D#?JL!L$$?+=J^`aOoSHa<0vCaCm2CBnuQt&a}2y+xXfS=L|QNW`&I1> z=itb7$NGl>9Y9#G&K|8i^~_R>>cG_0j)8&Mzkh$#w+@~-UeRo|ONBtR=W>uzXx_S#DH57%N1 zYl@g&iy#uwbrt&F`i8LwP_De?@g}(ILeunzxtPJ=5r(eO8rz=;9iXKYltxBULStN7 zesy>>c84(;6w;FB8e*M|ID8t&xBlHo#S|4r1kLl>cIb5>=fZ3#g(etiCyJW6zQex3 zPKE>A0Hv1h8FM~2zE3d~I%j%T8=Z&%(qPlXZBjyomUT+F&hPb$)B4*aig%flwtHLX zC=rUyQC4?de?YR_$9r`T*!uBs#e!CY|B87$1rv+4PwrNShNY( zd8VtajSfGvnaI>Jd`_*a|63^=$(PYVr*gkU&)jlNvNcb8_O{&f{9ROtHeFX+CD|uQ z2t2%h*;$akh@-=#6x%s}s=AgBd_}UqC?jgkTW-GhoEl5|Lr>$k(fW1 zFh?hkOHFmW$$fS8+vC}W-eBdRl{NdyZ*6~*@Z-N_8Z^q5O&MuW~;9DB(PJEhStFs<8fSCYNfsOZsE$HyjgF}GtJur z1!`n&+S*Tp#+p^i;i28zN4EC6I2V-Z!d6u4#pUarIKs<%(T>ZTpa@1g7u)x;!vm+= zEJ&0cUL=Fcv2-+i+r1bs9b%2I{~86g0Tgp=5mm*St(7x^hAp=@8;q~dy{w6aEr*d) zJ2S5VRJ<;?#~lIna25ky)5?DK6Hs=)$=-B>*YCs&dre+`lMd^sXS4GO%c4pocv|#l zQXZ=cP zc@FUOklmJBAvkr0m`UQGr!{eW#>t7>?tEr}Kl|&^BqOf_p<;<_&)uQ^{{EhOcq7|P zf?gl!wO(HoeK0%WN{khR(L~JM+w{i~`lC$#0hIA3Ta#Naivy%I_BibtG}2fTSLx_l z=a2gr?lGn|W_oxF)Omx_n!n_S&lMn+<+Cv|m@Aj)?7m!&kIwLKqw@U3sG2C+5_6(L zE2$hOc^Jzn28vv8r+vEIx`(THh9%oa#1D1GC51nphsQ0G%mi`T@a_nJpqiDnwL8i` z7IV<7%Z@*`)h%EkF@`!iJBC7qO}p>f<_*G1n<1OutxSdfJCxm&q>C`3mu`Z~EDwBL zSWiQvakt-XMl#`(89MGw5y(zTB);tP8oo?ZTUdSJVx9>_WfpifHUEw8B9q)nI~E|65zLv@}<#*?PpOcgW3? zE2G=XqMRBT^V{dTAIH-EZK9Fw5; zP9RZ%_XwjHhZ4E<=fygdr^Y)%tR!uH%SoH&CB$!GDsp2^elv6GLewY0$;(@1GYXYy zd_yk0>tYv_Ea_1T+|cUHg?RlqzU%gIJ3}zNUg_Pkj;h>ju{&3Np0B!wLuVQ^aR4xr z%EO1p1)*e1V&b#FMSbxB05Ps0d}yL?yGy7N_!IsV;9d4f(I0|URPL{NW!5Nd|B=~z z>+tj!W}qXKF~NNZA0j4V$;PEmpc$Fq=e^q&`i4ZRP(zIFRMt2x967!~pWyjXmlXE* zuk@kpR+rGnYZ(M@C+v9BOzB-XV;wf(1U%6HP_F}H{eSeNfN?QzK#t7YTc z7^n*QUoBMLE;;r!m_M1JX?DLb>+}d>El&QQPydgmt51{_8_z{Zz)-J5oxS56?#keU zr!ojZI>d`W0@RI;6n_f-`%7TQ00_)mxRjvQpU4b?4>SGRV9HOF_k3pP3H#Xy!36L8 zzFH^!_ZaAb9hm&twj_HQ^LbQO~_|m5WpzFnvFSY85155v{1D}!{D2A z5dRBXrDDpIu`8}F&8kYmy$GD4L+Jf$IGy)NL+NTPF$@!8U{XJ01KCgK0?eTL$JY(E zVMOo)qnLk8oJ$%kHZ%|tQ&0OES(o?^=b ze8`k=X+SoABFr$crt)P=WugY(PG0lL$s;5xEe;Ux(#*zYYqr73i;+BCV#+Ja!PeB$ z60BLG!L#B8g;mgDbSrwR!zX?S8T&#$=-bPW%`KulQhd3@SGg7W zcw7Ptf}=M2KYW#l9;JPG5+Z0+JiP6$`Kc+4jF1137OkCM6VpslqmXm>zjQ`J{hO6_S z&eiVn&Ix)9J@Dnzl?N0;{V#o#BH1Oz-<>3^0J@;|0t@{g5b(d;4_f%3_CHhuVC66g zsQp0^_7s@y|8lEH>mv-b7Axn}Rq6%QpHIL-{$DkCiYS8)ww4D`kc;@g@BGBYLNo$X zONLDC|JpeE369zMCtd0_wDEVyp(d;UWh5#os9O5}Y=t-ZTR`|u1xW69`d|b8XE$n& zNg9L?&}sT#Ci-8*&p>Y!>f`C&{*}ko^573W7?8vp0Ia&Y<++ycG6Zy?pNe!^9#FXh z0og7w0!bfB&~b~@L+GS-+(2+OFBtA{_Z5`TkeUVE4E!Q=+_aR?Z~D*UkD^CHvHoEs z9x+qw)odu%Kj4pGZ@u#Hg?5qC5hG9&s2EbrjXFc(c|KBYjES%E>A$jk|76-%D(5%i zaS$$=eP#;!SAdPpv*KF_P}+V4T5FH#@t$*-3f)>;4|CUe%!KEs}$>G#5yc;tM^M}|89-)OIAz1Opf6rb16vg@%??d32SG;+?y!RHu;wcdy(`1M{p|saCa+k@YCM$UY&Sj0Wu(gfurwF#EHC7r zDisTB`gkZ(Qho|3;~3()(GrY+F2j}wpRgcIChVklMW43Ja_>K~-r8ck4kck?P4^;b zQw0;i4${&+l{JM|^`zx1&+qY7(m;R~l3e5i5(DSsBRN*YvHh`a8>F@c*OZ`KG$Iwwvc<;`;i!mooa?`7MdH(QrFaw+}7n6-%U)zc4s=@Ba1` z6&>2WbTUYG0PVlNACHaC3h3{{(A6|utgXWwB1fHu=Rk{il%IhF%-A_1IvP=RUFzU% z^XAPPE2|gT!vv~)nRRyjiE<5PS0Hh`skd-BzJf@$n*nJ=>ibDea`Su%0QQSpeLE9=3D6`Z8cUg340vN5K8vGXHOm27%ec@88TxpDneeb3G-n3-$hyal92Ux8{b6?)kgL)hO&7JhEUBjEkOBvZC^YK1_HWI-cu zIUO4^>}IBHwQxaAhzY%8bF+vEF)Vz0O>E>?kxD6&ACzZ@}3XD4jwBogy)2usKVX} z1d1&N5*xlVa9cgL;>nz!pGogu!Dt-vNWZ}(Lj8ENQGWG~T$)gFyu(#-|4eCLAX@O9 zhlhuq=_nDJ0b)gBge_v>vUK6Hsr8EAl~iQ*UOX`HUJha5+xzhS`_Vl00?!QUth6LdNjg?Pd8b-v>bv z1+iwYJFvJvWDDm#ls6-ki8~&Gxpq6-MD_KfMSNvzlB#AV+m_5WR|6xaanrnTdt=aR zFg5FQ{C2KtyD+rxAfl)rUw)M8G{!jGZl=yIJV(2H;zsKBG4-1V%wcaB5{r`0ES?vb ztGZG+T*)3JXTyDvfyD4zBc#0IsGl&t$TF0Z?ObeFW8tKIU#!i^_((SCk?KI<23>Uh z5eEm8mfZ{ys)>U`w84VoZVj#MnDxnGXK;IuGhe1sf#5I=wWKPlCJq z6Fi!cW3P?+9K+o>lE#_r{h7Cwdyc;Ami9<~;L8Nu0x$Ejrp$pXh)H%UnsO$8$`0}w z(9*?4G{@Ae&K6%smxSfX6siaM>AiSeq{&=>hZk-pY@<`)e3V?$?Qk|+_uUp8gpti; z9odv8luicq=U-|p(^lkM-n^kc-)B>BigmdA)|9P~DJY%x0j$leT1PoStuTQ}Zq#&% z!M#a2tRdq$YDdDfG}FYdfmW;8Z|4 z$$X{`&6FpevN-S2u#3cSW@@U*!n1P_ABzrRz+?-GioSfsI>a#bkiuu^DWgJ<+Vbd_ z$g6_CU;9pox_C4!yrzdE+>f*2B{aONsya85`4~aaQ1-{1J=K|S#|RS_3X*6u%eEdN z@CAR3Zfx$d;HBoL#+l=XpyuR@*sY~nW-X>q8xQBw%jJ%|Bkz5m5oVH+nrii`j0%z2 z`cQ-wQus}ERh1(M{##Dy0d_Qbdg=7v#ISx#{;V<{P<$D72!lDwFS9 z+Pp3vo0~HLIr90kiLFFANy2WB@U~kAW?NdY*6T)R?$btj;dmZfFkZXGXVxHQIq<$c7 z_l})Toc=_$-OPyqM*23)pz|Bp#xyE)KdLyY-`3nsXF23E+KwuI* z_Bu0gy?ohcgoTUu8x)}5PANzYubJyuTri<0^jX}<8t!;}nWV+!*defxH5+h@wFf>CB40V;8; z9VHy6*@}dct}$aZ9?ehD{g@DOv|!`mm?9FuNyfaryQ5ro7$Oj@KOtW*>hsQ#SUSp^ zm@Cs_K~^bG%`q%3e1qU0Pk+Y*R47yPy)2&UM_~djB5RH7u?wF)$)20In{vVzsoeIK zVk-0of6_m&Sh4}2H4zy<5kIPPGTX{AIdWG5x>LF>Gkea*zya#BFouaqj9J&}mR3En zQXEBvWUkd^&K|B#SFTudKU@yYw~AWgd>q*}8XqS&W~-;Ec_=V&>Zgsxlqjj%g?t6W z_50(V)m!!Rh`d*}V6<#&vr$Q+LEqlicA)il-3}PJd;)p_=3)nvF+p*ZmfAI3s*^1w zWaqC=@t9J!(2fyP*x$*?l%-c-+Dy5=Duph-QNiIOPDRsU6x`B}d0_b@B}p#i!QN5B z7S+HokbF_rwQ>@5c4R^Cx36A}S#zCE2IEjx1rVW%@_nMmXt2*yQ`M|hU1+VOnTd;s z=Wv6!jKG^gc%Pm@K=4>EjVUb0ZsKtijeMN&RDa!0WF(f>iBz}pdT%tP(YJT)3la*r zsKZB-!MBn3{%dkXkRMdnvmotnzD0+oy@!WaiBf0-TF~n_I7q4QZtxfOq7#LGS{iIYf>xZe2gWVbQYX?2gIC2 z;QUynjmbUB`|>O7ue)KMu6s=ja=9-a0;gfCB9qYt?TYtP( zqsxP(UeF|zL$9Qukdc;JIA)z_-Thv_*D54uxy$LkEY!hD3#11^9SKalr|<96yT=Oh+G(U|pI9ok2M2^#8;s5(hJ+}zjF zcO>2#H-#88+WN+aGD~3$yBJKoC!csmJyW+;*IeDSr95Eqh+}E(Wv3S1l_lxv(2~(o z{Z$_zDO-(MYrIjQt=*~@b4zB}lOaU`Hj_^j7O=gy!3CN7i+qy?+4V#?-qlA1Vi=JVeC zU8wLpzRxUeO%F{yHLnB?8ovLW=m%6YPMK!Ik?yWkvcJ@o1rvwY=;-KrdqKOIww~TY zr@a@xrcm2aj(b+JQUOP*0!wnWot|n!Utb@{gl@hr+r$zoG8h?H#8@%Ts74XRWlxP* zb5F~!H1AK#myHYfY#!3t)|2>h<{_-(ok6l@PuUjWs#3X&uaqhVPNJJ(xALx{e=b5Y zt=(tw$@1!$Y#bbhY4A}$Hz?c>!LWu+*W$+d`wJV45Ux+!Dz7tdtJvl{rH0wi#3@pQ_ruC4;Z@#d+hx?ZcK zS|!TL%J7>Am>`s&#RnuopT!M%?T#NHO_u&b(!yk;ad&7a%3R0CtV&oX>fXg-KWBkU zMK8;TN;ng`*z@>GyP2c!f4HCHXtn4Y!EJTM8=TgDMq`aY8A_QP2cM(#RE5(rg}eV2E4 zjJB(6u0NUIN48KS)zPv)@~^zYo)L|(d)o1MYCc=hg-gWj9&cHM)gBx51|r5ji)v)g z1d~3RC@xMst!x}EIH~Xde5Zesa9@ACN;Ot*up0ZF39@ED6ViD2zIj_a1%E z%%tF3U$MXe4jFIHfEUImlZR_l{ zaZxojZxDWuO3if&l(XyX((O_=>Oo5(UhJBCl6hf~b(%$~mT9}~OkKWJ2}*|G7&3Yh zuXgA3-a1rM$5%f0z$c;@m}gg1AFW9?^e8keuQpk2L(x^y96J(_RMj9`$E)=p#Xb5w_34T`D&3t@F6R%Wo!(vl*lo0}ACHgw#C)iIK zXiE&_@Iy|kM_3J~Xqi+AZ^QuG-fgqg0nXT#g@oVUJrchCy6?o}W{WXI9{{I>Xw0+k z?qW>)9d~^t2v9|-Sid>lX9TZ*&A#Ql7G%Bou*z36mGb_1d}>ApXtge%e3?!|)TMhZ zU1|08gaZb{mxBH;fafbmaKyI~>7HZg^NY|zT+(^Tln>|>Ew~x1Z_Pc2zG{9I`#LzF z{R#dyi6!M-?5tiIYUbpvQ!C}WP;KFpC&bDlK<9Z&VI-jOu74~)uem461$NUliAvTG zN-mxb&LU6yWNBi5fKKOaEeY#kb+KraX6{0MSXpYW5ew4 z--Mp`WaH9^U%FX9QK+vL$W)5_s_giEB%rA|47x=@ieSdn)HG|2g0R+S)|EJEM?0z8 z(Je>iYZh;(RI6rC)@YLjFh>HN35)%|VI5P#=%~Va z@dXYWpS%+4=;`SJnW#mG=fu%QMn@CG(xZdX?>?r|^;ZCSN4D#0CxZ!>IjLOLa*(2{ zWMa%z;(3gf)IIjNwYGL|k|*&{VE8@x@DX61*hJEmWTyMccO~xp)2r}I_)b+{l%%RN zxpb-4jn3=sq$p;z>{7;(rRm!^il#C$N4~TFq#pS|l1;?O&d%;YQTjkVW~{_V!Ga<2 z!01-@<)HX$X$iq^8)Ev=Qwu<1nkN+ zY8;p+Nbg!*yZM}eE3=n~-5s+M(!j6Ew}g@}U-cJ*;L&QB5aYrNxg|gIRjU{{ z!`*VYL7_&LfO|)_eVkIk6!~5fXs1&JkPS`^<{hH2+RmdKj(pyeKOZX^NNsH`9c;Fy zsx8Iu6;;>s6ws&TTcxe?VEBPJMt-ZUwgO5)|L1hd@CipWkcp+F4xC;b)gBxeoAM_r z^mt!dwbVxTDXb1$^5MOw`B~zOEqtHslit6Og8LvWWO9B2Gd;y%(2P*>0Y}a3BgSMs z&y8n%*A$^4b5&wV<`FF73jYvemdU17iQ)$awkYoD>FLiXdzbidE|^TCM(DiYqQ@l!$dZBNO{T1}tEMg8bPQ7~%N`X1gj z5$+~c(@M{n@-tEjMQ{?A078$?7Mqyi(9Q#MgAKfhn|Mu(`lt`z0^Q7MR8 zK$Jus{lh5v8K7?NFlwGZ&CM`a)bVm30rwUINn6xXMG7WwV@VOiRv`i=i|x@%NLqGc`}vuc6WV1mOE>$uj+e!y=B^} zw&6r?&-r}`@Y=O>^b;c(PTe|YXLXgxh-)hW|>*nm*a?i@Q0^Kbxb2_rqToE1T-}0>hT^n3+r1U<< z^CSJs_6L*_Ua2u8s;OO7mjRL?ILz;|nP8O$a3gqvKw4gN45S(R%q+lL^ffIkeN{Rv z#P#)LQ3M%o_s9{Alg=m#=nwM%BxbD%>gyG2F>oOq4S2EXXsadgGALkuP~D%_z~qe>Za_jDbjyMGiUvK0R`xE~@>*TRI9jB&FTL}DA1JpNKpaK5O$m~i>E0hBVdb_B@e@*#t- zy$kg8`W%EC&VibvF56^+JtOt%tm2v%ord%NYCO49exJ z2)~bVhy>ir=r`~lLi3H69W?u`pf$>=V^LkMRs-)f|0rV|UwP46^Ow*52Xiw?-36c` ze)JfA^yq#f337S*y^&VDL@+TtSez0eDmR*L94%KS5$b0Z`idWzMwb|VR3T!o#SzqA?Or^XS3<2FMg0Uk0=4po#7auzKCj z&d+DT0o?gW#H&DI6sArliZkQ(LqeFb3aTjXGub5Cj{!ABRYcVIGPnW&&WR>70Rt2J z9MflZPEcBrIN0u|O+}60=cWrjcE5!$38qkDryKp9w!Y>6qlj~lXL|qR z_>@j{lH`z;OOgtaOC!0)&pM4(skTGqZnen>O%Yvm5Q>^xb3F|!TTM-EtfYu#%dNSL zg?<%F(@{E;Tm0VZ{PFw#vB$Q@_Wk4Y{eIt{_xt&Jz288=9J<4BTAO+m5_*u4k(;3= zm&vrEsx}mouK=syAE}`>YjL=(x#`9j z5^i8y#EGuGjZwm3&aRw>8k-kNc>p7;EipV`w&}MUa=xiA#BJE)!Q@Y_>xHRwol zZqGKZ>?9s$b?=Q|I9kJQ97)iBy2fzbM`u_MFP2%HhXJYYZeFWUnIz>A`nd4a^;0(U zPRjJ+Xmk@2`s)={x9g#m!36-wq=&bGz2x8I;#nZHXF#e{KxpBFODDTNK-G21Im^N^ z){oaoJbfq4x&7ps$(L4Z&<`LmnQ8Y@?(J^mlI`N_{;)e_>f0-rT8dB(yg^`iCy}Fv znS;ca)X-iz)r6tdgldJZoNf8qXcB52=grK(*vo&&kW-KfN@O?|&NmNO^P_M)l_0+; zXYl(ssu@1%sR}_{{2&<^=>9gnyDSKAmPY{i&3|v7DJG)ArFLo?^F?X&m%H<`qIPUS zdrS89Q;p$NVw7Al)7GZGdgn?36ziRLeDQVuRlV|S@qU_3>7 zir9j^hiAQvF6+G0$2Cu*8ZW1R%ROucwUutPgRqb^DU8FHgKcxX6ZEP6gor z0CavA=Z*<2Joel<$Ly9axgxqYykO+jE9fL56rQu9o6IEkA2X|7Tz{ev*P^7`@;?C9 zB%V3b%Q5HPxWXU_UwH5jw#cG^LM?GK$rp`z5|0;T@Y{B{D_vE4_{GJ=?zavofgOk< zPMfGUXU!6&TQG2$Po4sZ-gNZ|vb2X%=VQ)f9$|Gm zwI9-IvQo1<|0;<_yub?M>L4F)M4RP8^_K)N-PVYC*v#XDe&A4(D+n^Zl-NMuJYx)# z$!8zkme1pILFlCYSiizB?DBrX7^oq?bI6YQqU9x9G=WH9#V~8(HLPK|n6%)PqRt!y zvc|!CYYC#}u+@FG7R!^adsaxS&*hM%xt@gJuurdO+p`Ru9TIjz;adt?r1ErytuFm< zoX*Xlq#9Do80YqoLse zBROc#cxwF{;5CZ)?~cXk^nE*d=B!@%Ff9z(j3x^fHCePce%M-X!W^agcMT7e=fs(9 zZ=NV<1&S$Gps?s$r$%N6BJ%rH*oy}Z7C&2e0Qaab{Jh?^k-h+eUbbx*)w z`&033=ML;42CZzF>2SDRSs-IIdETMGKiG08;j4C&MIz(RwvfVxoBgCnZP^=W8$^|v zFp2N7crc|Z!bLT8KaO=`T`W2)=pKmAym;;L|}Qs#R&A&}S2*=g_Xee*geNDdo9t*JUog{Lvn z(uq}9AE2RADPUcRkIWoxnoch$sgKXVO44&u+)| z&5dw-iKB|_GxY_xO5Cs9yJyjiEEZt$Vnc^*siUe?6xKpSU+L|p(G@wsp@aw-<1=+X zE-&uaQ+(=X&BgwE=_t|h@%+b+9g%ABjWOL@KQTx8GutFY6rB{BSRE7XmQfFq=$ee24&S`wAp6qx zWQ~KKXzzFScpProYC)8q0Kz0&&OKr=^gW>dd>xOMimp5rf;Ye`xZ%^2k4`(xW=~3aoE;e@;t0R$(YP7LjDBG&CZ7 zw71?6`n+3)Siq%x6-EE{OtwKC{6hGjqZR{kKTUf-KSZqa)LNXN9J_&F@2))k #include -#include -#include +#include #include #include @@ -78,7 +77,7 @@ std::optional calculate_pose_ahead_of_collision( const auto interpolated_pose = motion_utils::calcInterpolatedPose(ego_data.trajectory_points, l); const auto interpolated_footprint = project_to_pose(footprint, interpolated_pose); - if (boost::geometry::intersects(interpolated_footprint, point_to_avoid.outside_ring)) { + if (!boost::geometry::disjoint(interpolated_footprint, point_to_avoid.out_overlaps)) { return interpolated_pose; } } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 6febd15ef8052..7d845eb788ef8 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -21,11 +21,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include #include #include @@ -36,6 +37,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane::debug { @@ -151,53 +153,59 @@ size_t add_stop_line_markers( } return max_id; } -} // namespace -visualization_msgs::msg::MarkerArray create_debug_marker_array( - const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, - const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +void add_out_lanelets( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const lanelet::ConstLanelets & out_lanelets) { - const auto z = ego_data.pose.position.z; - visualization_msgs::msg::MarkerArray debug_marker_array; - - auto base_marker = get_base_marker(); - base_marker.pose.position.z = z + 0.5; - base_marker.ns = "footprints"; - base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); - // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation - // disabled to prevent performance issues when publishing the debug markers - // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); - lanelet::BasicPolygons2d drivable_lane_polygons; - for (const auto & poly : ego_data.drivable_lane_polygons) { - drivable_lane_polygons.push_back(poly.outer); + for (const auto & ll : out_lanelets) { + drivable_lane_polygons.push_back(ll.polygon2d().basicPolygon()); } - base_marker.ns = "ego_lane"; + base_marker.ns = "out_lanelets"; base_marker.color = universe_utils::createMarkerColor(0.0, 0.0, 1.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, drivable_lane_polygons); + add_polygons_markers(marker_array, base_marker, drivable_lane_polygons); +} - lanelet::BasicPolygons2d out_of_lane_areas; - for (const auto & p : out_of_lane_data.outside_points) { - out_of_lane_areas.push_back(p.outside_ring); +void add_out_of_lane_overlaps( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const std::vector & outside_points, + const std::vector & trajectory) +{ + lanelet::BasicPolygons2d out_of_lane_overlaps; + lanelet::BasicPolygon2d out_of_lane_overlap; + for (const auto & p : outside_points) { + for (const auto & overlap : p.out_overlaps) { + boost::geometry::convert(overlap, out_of_lane_overlap); + out_of_lane_overlaps.push_back(out_of_lane_overlap); + } } base_marker.ns = "out_of_lane_areas"; base_marker.color = universe_utils::createMarkerColor(1.0, 0.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, out_of_lane_areas); - for (const auto & [bbox, i] : out_of_lane_data.outside_areas_rtree) { - const auto & a = out_of_lane_data.outside_points[i]; - debug_marker_array.markers.back().points.push_back( - ego_data.trajectory_points[a.trajectory_index].pose.position); - const auto centroid = boost::geometry::return_centroid(a.outside_ring); - debug_marker_array.markers.back().points.push_back( - geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + add_polygons_markers(marker_array, base_marker, out_of_lane_overlaps); + for (const auto & p : outside_points) { + for (const auto & a : p.out_overlaps) { + marker_array.markers.back().points.push_back(trajectory[p.trajectory_index].pose.position); + const auto centroid = boost::geometry::return_centroid(a); + marker_array.markers.back().points.push_back( + geometry_msgs::msg::Point().set__x(centroid.x()).set__y(centroid.y())); + } } - +} +void add_predicted_paths( + visualization_msgs::msg::MarkerArray & marker_array, visualization_msgs::msg::Marker base_marker, + const autoware_perception_msgs::msg::PredictedObjects & objects, + const geometry_msgs::msg::Pose & ego_pose) +{ + base_marker.ns = "objects"; + base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); lanelet::BasicPolygons2d object_polygons; + constexpr double max_draw_distance = 50.0; for (const auto & o : objects.objects) { for (const auto & path : o.kinematics.predicted_paths) { for (const auto & pose : path.path) { // limit the draw distance to improve performance - if (universe_utils::calcDistance2d(pose, ego_data.pose) < 50.0) { + if (universe_utils::calcDistance2d(pose, ego_pose) < max_draw_distance) { const auto poly = universe_utils::toPolygon2d(pose, o.shape).outer(); lanelet::BasicPolygon2d ll_poly(poly.begin(), poly.end()); object_polygons.push_back(ll_poly); @@ -205,9 +213,28 @@ visualization_msgs::msg::MarkerArray create_debug_marker_array( } } } - base_marker.ns = "objects"; - base_marker.color = universe_utils::createMarkerColor(0.0, 1.0, 0.0, 1.0); - add_polygons_markers(debug_marker_array, base_marker, object_polygons); + add_polygons_markers(marker_array, base_marker, object_polygons); +} +} // namespace + +visualization_msgs::msg::MarkerArray create_debug_marker_array( + const EgoData & ego_data, const OutOfLaneData & out_of_lane_data, + const autoware_perception_msgs::msg::PredictedObjects & objects, DebugData & debug_data) +{ + const auto z = ego_data.pose.position.z; + visualization_msgs::msg::MarkerArray debug_marker_array; + + auto base_marker = get_base_marker(); + base_marker.pose.position.z = z + 0.5; + base_marker.ns = "footprints"; + base_marker.color = universe_utils::createMarkerColor(1.0, 1.0, 1.0, 1.0); + // TODO(Maxime): move the debug marker publishing AFTER the trajectory generation + // disabled to prevent performance issues when publishing the debug markers + // add_polygons_markers(debug_marker_array, base_marker, ego_data.trajectory_footprints); + add_out_lanelets(debug_marker_array, base_marker, ego_data.out_lanelets); + add_out_of_lane_overlaps( + debug_marker_array, base_marker, out_of_lane_data.outside_points, ego_data.trajectory_points); + add_predicted_paths(debug_marker_array, base_marker, objects, ego_data.pose); add_current_overlap_marker(debug_marker_array, ego_data.current_footprint, z); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp index 564bf5de7ef2e..6e31f8c8455fd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/footprint.cpp @@ -18,6 +18,8 @@ #include +#include + #include #include @@ -37,6 +39,7 @@ universe_utils::Polygon2d make_base_footprint(const PlannerParam & p, const bool {p.front_offset + front_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.right_offset - right_offset}, {p.rear_offset - rear_offset, p.left_offset + left_offset}}; + boost::geometry::correct(base_footprint); return base_footprint; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp index f520a564519f0..e4840d724e43d 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -16,16 +16,24 @@ #include "types.hpp" +#include +#include #include +#include #include +#include #include +#include #include +#include #include +#include #include #include +#include namespace autoware::motion_velocity_planner::out_of_lane { @@ -77,15 +85,13 @@ lanelet::ConstLanelets get_missing_lane_change_lanelets( } lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler) + const universe_utils::LineString2d & trajectory_ls, + const route_handler::RouteHandler & route_handler) { const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); lanelet::ConstLanelets trajectory_lanelets; - lanelet::BasicLineString2d trajectory_ls; - for (const auto & p : ego_data.trajectory_points) - trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); - const auto candidates = - lanelet_map_ptr->laneletLayer.search(lanelet::geometry::boundingBox2d(trajectory_ls)); + const auto candidates = lanelet_map_ptr->laneletLayer.search( + boost::geometry::return_envelope(trajectory_ls)); for (const auto & ll : candidates) { if ( is_road_lanelet(ll) && @@ -115,51 +121,77 @@ lanelet::ConstLanelets calculate_ignored_lanelets( return ignored_lanelets; } -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler) +lanelet::ConstLanelets calculate_out_lanelets( + const lanelet::LaneletLayer & lanelet_layer, + const universe_utils::MultiPolygon2d & trajectory_footprints, + const lanelet::ConstLanelets & trajectory_lanelets, + const lanelet::ConstLanelets & ignored_lanelets) { - const auto route_lanelets = calculate_trajectory_lanelets(ego_data, route_handler); - const auto ignored_lanelets = - out_of_lane::calculate_ignored_lanelets(route_lanelets, route_handler); - for (const auto & ll : route_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; - } - for (const auto & ll : ignored_lanelets) { - out_of_lane::Polygons tmp; - boost::geometry::union_(ego_data.drivable_lane_polygons, ll.polygon2d().basicPolygon(), tmp); - ego_data.drivable_lane_polygons = tmp; + lanelet::ConstLanelets out_lanelets; + const auto candidates = lanelet_layer.search( + boost::geometry::return_envelope(trajectory_footprints)); + for (const auto & lanelet : candidates) { + const auto id = lanelet.id(); + if ( + contains_lanelet(trajectory_lanelets, id) || contains_lanelet(ignored_lanelets, id) || + !is_road_lanelet(lanelet)) { + continue; + } + if (!boost::geometry::disjoint(trajectory_footprints, lanelet.polygon2d().basicPolygon())) { + out_lanelets.push_back(lanelet); + } } + return out_lanelets; } -void calculate_overlapped_lanelets( - OutOfLanePoint & out_of_lane_point, const route_handler::RouteHandler & route_handler) +OutLaneletRtree calculate_out_lanelet_rtree(const lanelet::ConstLanelets & lanelets) { - out_of_lane_point.overlapped_lanelets = lanelet::ConstLanelets(); - const auto candidates = route_handler.getLaneletMapPtr()->laneletLayer.search( - lanelet::geometry::boundingBox2d(out_of_lane_point.outside_ring)); - for (const auto & ll : candidates) { - if ( - is_road_lanelet(ll) && !contains_lanelet(out_of_lane_point.overlapped_lanelets, ll.id()) && - boost::geometry::within(out_of_lane_point.outside_ring, ll.polygon2d().basicPolygon())) { - out_of_lane_point.overlapped_lanelets.push_back(ll); - } + std::vector nodes; + nodes.reserve(lanelets.size()); + for (auto i = 0UL; i < lanelets.size(); ++i) { + nodes.emplace_back( + boost::geometry::return_envelope( + lanelets[i].polygon2d().basicPolygon()), + i); } + return {nodes.begin(), nodes.end()}; } -void calculate_overlapped_lanelets( - OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler) +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params) { - for (auto it = out_of_lane_data.outside_points.begin(); - it != out_of_lane_data.outside_points.end();) { - calculate_overlapped_lanelets(*it, route_handler); - if (it->overlapped_lanelets.empty()) { - // do not keep out of lane points that do not overlap any lanelet - out_of_lane_data.outside_points.erase(it); - } else { - ++it; - } + universe_utils::LineString2d trajectory_ls; + for (const auto & p : ego_data.trajectory_points) { + trajectory_ls.emplace_back(p.pose.position.x, p.pose.position.y); } + // add a point beyond the last trajectory point to account for the ego front offset + const auto pose_beyond = universe_utils::calcOffsetPose( + ego_data.trajectory_points.back().pose, params.front_offset, 0.0, 0.0, 0.0); + trajectory_ls.emplace_back(pose_beyond.position.x, pose_beyond.position.y); + const auto trajectory_lanelets = calculate_trajectory_lanelets(trajectory_ls, route_handler); + const auto ignored_lanelets = calculate_ignored_lanelets(trajectory_lanelets, route_handler); + + const auto max_ego_footprint_offset = std::max({ + params.front_offset + params.extra_front_offset, + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset, + params.rear_offset + params.extra_rear_offset, + }); + universe_utils::MultiPolygon2d trajectory_footprints; + const boost::geometry::strategy::buffer::distance_symmetric distance_strategy( + max_ego_footprint_offset); + const boost::geometry::strategy::buffer::join_miter join_strategy; + const boost::geometry::strategy::buffer::end_flat end_strategy; + const boost::geometry::strategy::buffer::point_square circle_strategy; + const boost::geometry::strategy::buffer::side_straight side_strategy; + boost::geometry::buffer( + trajectory_ls, trajectory_footprints, distance_strategy, side_strategy, join_strategy, + end_strategy, circle_strategy); + + ego_data.out_lanelets = calculate_out_lanelets( + route_handler.getLaneletMapPtr()->laneletLayer, trajectory_footprints, trajectory_lanelets, + ignored_lanelets); + ego_data.out_lanelets_rtree = calculate_out_lanelet_rtree(ego_data.out_lanelets); } } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp index a7729deb539b6..0cb9e223c6456 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -18,6 +18,7 @@ #include "types.hpp" #include +#include #include @@ -43,7 +44,8 @@ inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lane /// @param [in] route_handler route handler /// @return lanelets crossed by the ego vehicle lanelet::ConstLanelets calculate_trajectory_lanelets( - const EgoData & ego_data, const std::shared_ptr route_handler); + const universe_utils::LineString2d & trajectory_ls, + const std::shared_ptr route_handler); /// @brief calculate lanelets that may not be crossed by the trajectory but may be overlapped during /// a lane change @@ -66,14 +68,18 @@ lanelet::ConstLanelets calculate_ignored_lanelets( /// @brief calculate the polygons representing the ego lane and add it to the ego data /// @param [inout] ego_data ego data /// @param [in] route_handler route handler with map information -void calculate_drivable_lane_polygons( - EgoData & ego_data, const route_handler::RouteHandler & route_handler); +// void calculate_drivable_lane_polygons( +// EgoData & ego_data, const route_handler::RouteHandler & route_handler); /// @brief calculate the lanelets overlapped at each out of lane point /// @param [out] out_of_lane_data data with the out of lane points /// @param [in] route_handler route handler with the lanelet map void calculate_overlapped_lanelets( OutOfLaneData & out_of_lane_data, const route_handler::RouteHandler & route_handler); + +void calculate_out_lanelet_rtree( + EgoData & ego_data, const route_handler::RouteHandler & route_handler, + const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp index aef035200b6cc..176b0d2aed2b3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.cpp @@ -20,15 +20,15 @@ #include #include -#include +#include -#include -#include #include #include +#include #include #include +#include #include #include @@ -47,7 +47,7 @@ void update_collision_times( auto & out_of_lane_point = out_of_lane_data.outside_points[index]; if (out_of_lane_point.collision_times.count(time) == 0UL) { const auto has_collision = - !boost::geometry::disjoint(out_of_lane_point.outside_ring, object_footprint.outer()); + !boost::geometry::disjoint(out_of_lane_point.out_overlaps, object_footprint.outer()); if (has_collision) { out_of_lane_point.collision_times.insert(time); } @@ -129,20 +129,39 @@ void calculate_collisions_to_avoid( } } +OutOfLanePoint calculate_out_of_lane_point( + const lanelet::BasicPolygon2d & footprint, const lanelet::ConstLanelets & out_lanelets, + const OutLaneletRtree & out_lanelets_rtree) +{ + OutOfLanePoint p; + std::vector candidates; + out_lanelets_rtree.query( + boost::geometry::index::intersects(footprint), std::back_inserter(candidates)); + for (const auto & [_, idx] : candidates) { + const auto & lanelet = out_lanelets[idx]; + lanelet::BasicPolygons2d intersections; + boost::geometry::intersection(footprint, lanelet.polygon2d().basicPolygon(), intersections); + for (const auto & intersection : intersections) { + universe_utils::Polygon2d poly; + boost::geometry::convert(intersection, poly); + p.out_overlaps.push_back(poly); + } + if (!intersections.empty()) { + p.overlapped_lanelets.push_back(lanelet); + } + } + return p; +} std::vector calculate_out_of_lane_points(const EgoData & ego_data) { std::vector out_of_lane_points; - OutOfLanePoint p; for (auto i = 0UL; i < ego_data.trajectory_footprints.size(); ++i) { - p.trajectory_index = i + ego_data.first_trajectory_idx; const auto & footprint = ego_data.trajectory_footprints[i]; - Polygons out_of_lane_polygons; - boost::geometry::difference(footprint, ego_data.drivable_lane_polygons, out_of_lane_polygons); - for (const auto & area : out_of_lane_polygons) { - if (!area.outer.empty()) { - p.outside_ring = area.outer; - out_of_lane_points.push_back(p); - } + OutOfLanePoint p = + calculate_out_of_lane_point(footprint, ego_data.out_lanelets, ego_data.out_lanelets_rtree); + p.trajectory_index = ego_data.first_trajectory_idx + i; + if (!p.overlapped_lanelets.empty()) { + out_of_lane_points.push_back(p); } } return out_of_lane_points; @@ -152,11 +171,12 @@ void prepare_out_of_lane_areas_rtree(OutOfLaneData & out_of_lane_data) { std::vector rtree_nodes; for (auto i = 0UL; i < out_of_lane_data.outside_points.size(); ++i) { - OutAreaNode n; - n.first = boost::geometry::return_envelope( - out_of_lane_data.outside_points[i].outside_ring); - n.second = i; - rtree_nodes.push_back(n); + for (const auto & out_overlap : out_of_lane_data.outside_points[i].out_overlaps) { + OutAreaNode n; + n.first = boost::geometry::return_envelope(out_overlap); + n.second = i; + rtree_nodes.push_back(n); + } } out_of_lane_data.outside_areas_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp index 33f0842c56d36..f0e0360ef1c15 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index c97e10cc8706e..347a138b651fa 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -221,12 +221,10 @@ void prepare_stop_lines_rtree( ego_data.stop_lines_rtree = {rtree_nodes.begin(), rtree_nodes.end()}; } -out_of_lane::OutOfLaneData prepare_out_of_lane_data( - const out_of_lane::EgoData & ego_data, const route_handler::RouteHandler & route_handler) +out_of_lane::OutOfLaneData prepare_out_of_lane_data(const out_of_lane::EgoData & ego_data) { out_of_lane::OutOfLaneData out_of_lane_data; out_of_lane_data.outside_points = out_of_lane::calculate_out_of_lane_points(ego_data); - out_of_lane::calculate_overlapped_lanelets(out_of_lane_data, route_handler); out_of_lane::prepare_out_of_lane_areas_rtree(out_of_lane_data); return out_of_lane_data; } @@ -255,11 +253,11 @@ VelocityPlanningResult OutOfLaneModule::plan( const auto calculate_trajectory_footprints_us = stopwatch.toc("calculate_trajectory_footprints"); stopwatch.tic("calculate_lanelets"); - out_of_lane::calculate_drivable_lane_polygons(ego_data, *planner_data->route_handler); + out_of_lane::calculate_out_lanelet_rtree(ego_data, *planner_data->route_handler, params_); const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); stopwatch.tic("calculate_out_of_lane_areas"); - auto out_of_lane_data = prepare_out_of_lane_data(ego_data, *planner_data->route_handler); + auto out_of_lane_data = prepare_out_of_lane_data(ego_data); const auto calculate_out_of_lane_areas_us = stopwatch.toc("calculate_out_of_lane_areas"); stopwatch.tic("filter_predicted_objects"); @@ -275,9 +273,12 @@ VelocityPlanningResult OutOfLaneModule::plan( out_of_lane::calculate_collisions_to_avoid(out_of_lane_data, ego_data.trajectory_points, params_); const auto calculate_times_us = stopwatch.toc("calculate_times"); - if ( - params_.skip_if_already_overlapping && !ego_data.drivable_lane_polygons.empty() && - !lanelet::geometry::within(ego_data.current_footprint, ego_data.drivable_lane_polygons)) { + const auto is_already_overlapping = + params_.skip_if_already_overlapping && + std::find_if(ego_data.out_lanelets.begin(), ego_data.out_lanelets.end(), [&](const auto & ll) { + return !boost::geometry::disjoint(ll.polygon2d().basicPolygon(), ego_data.current_footprint); + }) != ego_data.out_lanelets.end(); + if (is_already_overlapping) { RCLCPP_WARN(logger_, "Ego is already out of lane, skipping the module\n"); debug_publisher_->publish(out_of_lane::debug::create_debug_marker_array( ego_data, out_of_lane_data, objects, debug_data_)); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index c3714c5609135..edf4aff8bc85c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -88,6 +88,8 @@ using StopLineNode = std::pair; using StopLinesRtree = bgi::rtree>; using OutAreaNode = std::pair; using OutAreaRtree = bgi::rtree>; +using LaneletNode = std::pair; +using OutLaneletRtree = bgi::rtree>; /// @brief data related to the ego vehicle struct EgoData @@ -100,7 +102,8 @@ struct EgoData double min_slowdown_distance{}; double min_stop_arc_length{}; - Polygons drivable_lane_polygons; + lanelet::ConstLanelets out_lanelets; + OutLaneletRtree out_lanelets_rtree; lanelet::BasicPolygon2d current_footprint; std::vector trajectory_footprints; @@ -108,10 +111,11 @@ struct EgoData StopLinesRtree stop_lines_rtree; }; +/// @brief data related to an out of lane trajectory point struct OutOfLanePoint { size_t trajectory_index; - lanelet::BasicPolygon2d outside_ring; + universe_utils::MultiPolygon2d out_overlaps; std::set collision_times; std::optional min_object_arrival_time; std::optional max_object_arrival_time; @@ -119,6 +123,8 @@ struct OutOfLanePoint lanelet::ConstLanelets overlapped_lanelets; bool to_avoid = false; }; + +/// @brief data related to the out of lane points struct OutOfLaneData { std::vector outside_points; From 1750903107404cdfe70d397bdbe7bd970490592a Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Mon, 30 Sep 2024 16:51:40 +0900 Subject: [PATCH 09/27] fix(autoware_universe_utils): fix unmatchedSuppression (#8986) fix:unmatchedSuppression Signed-off-by: kobayu858 --- common/autoware_universe_utils/src/ros/marker_helper.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/common/autoware_universe_utils/src/ros/marker_helper.cpp b/common/autoware_universe_utils/src/ros/marker_helper.cpp index 378dc795a7421..0d360e2335df7 100644 --- a/common/autoware_universe_utils/src/ros/marker_helper.cpp +++ b/common/autoware_universe_utils/src/ros/marker_helper.cpp @@ -16,7 +16,6 @@ namespace autoware::universe_utils { -// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker createDefaultMarker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, const int32_t type, const geometry_msgs::msg::Vector3 & scale, @@ -41,7 +40,6 @@ visualization_msgs::msg::Marker createDefaultMarker( return marker; } -// cppcheck-suppress unusedFunction visualization_msgs::msg::Marker createDeletedDefaultMarker( const rclcpp::Time & now, const std::string & ns, const int32_t id) { @@ -55,7 +53,6 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker( return marker; } -// cppcheck-suppress unusedFunction void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, From 5a0e1003bab6ea617f2754f266fd8af3309a0ff0 Mon Sep 17 00:00:00 2001 From: melike tanrikulu <41450930+meliketanrikulu@users.noreply.github.com> Date: Mon, 30 Sep 2024 17:15:09 +0300 Subject: [PATCH 10/27] docs(autoware_pose_cov_modifier): fix line breaks and dead links (#8991) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix(autoware_pose_cov_modifier): fix line breaks Signed-off-by: Melike Tanrıkulu * fix dead links Signed-off-by: Melike Tanrıkulu --------- Signed-off-by: Melike Tanrıkulu --- .../README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/localization/autoware_pose_covariance_modifier/README.md b/localization/autoware_pose_covariance_modifier/README.md index b834bc88116a3..60ecf4bde3508 100644 --- a/localization/autoware_pose_covariance_modifier/README.md +++ b/localization/autoware_pose_covariance_modifier/README.md @@ -73,13 +73,13 @@ Here is a flowchart depicting the process and the predefined thresholds: ```mermaid graph TD - gnss_poser["gnss_poser"] --> |"/sensing/gnss/\npose_with_covariance"| pose_covariance_modifier_node - ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/\npose_with_covariance"| pose_covariance_modifier_node + gnss_poser["gnss_poser"] --> |"/sensing/gnss/
pose_with_covariance"| pose_covariance_modifier_node + ndt_scan_matcher["ndt_scan_matcher"] --> |"/localization/pose_estimator/ndt_scan_matcher/
pose_with_covariance"| pose_covariance_modifier_node subgraph pose_covariance_modifier_node ["Pose Covariance Modifier Node"] - pc1{{"gnss_pose_yaw\nstddev"}} - pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z\nstddev"}} - pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy\nstddev"}} + pc1{{"gnss_pose_yaw
stddev"}} + pc1 -->|"<= 0.3 rad"| pc2{{"gnss_pose_z
stddev"}} + pc2 -->|"<= 0.1 m"| pc3{{"gnss_pose_xy
stddev"}} pc2 -->|"> 0.1 m"| ndt_pose("NDT Pose") pc3 -->|"<= 0.1 m"| gnss_pose("GNSS Pose") pc3 -->|"0.1 m < x <= 0.2 m"| gnss_ndt_pose("`Both GNSS and NDT Pose @@ -117,8 +117,8 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau ### Without this condition (default) -- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is directly sent - to [ekf_localizer](../../localization/ekf_localizer). +- The output of the [ndt_scan_matcher](../../localization/autoware_ndt_scan_matcher) is directly sent + to [ekf_localizer](../../localization/autoware_ekf_localizer). - It has a preset covariance value. - **topic name:** `/localization/pose_estimator/pose_with_covariance` - The GNSS pose does not enter the ekf_localizer. @@ -126,11 +126,11 @@ the [pose_twist_estimator.launch.xml](../../launch/tier4_localization_launch/lau ### With this condition -- The output of the [ndt_scan_matcher](../../localization/ndt_scan_matcher) is renamed +- The output of the [ndt_scan_matcher](../../localization/autoware_ndt_scan_matcher) is renamed - **from:** `/localization/pose_estimator/pose_with_covariance`. - **to:** `/localization/pose_estimator/ndt_scan_matcher/pose_with_covariance`. - The `ndt_scan_matcher` output enters the `autoware_pose_covariance_modifier`. -- The output of this package goes to [ekf_localizer](../../localization/ekf_localizer) with: +- The output of this package goes to [ekf_localizer](../../localization/autoware_ekf_localizer) with: - **topic name:** `/localization/pose_estimator/pose_with_covariance`. ## Node From 90ba42b360291e424bcc6e24f613a41b72d03118 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 1 Oct 2024 11:18:27 +0900 Subject: [PATCH 11/27] refactor(goal_planner): refactor PullOverPlannseBase to instantiate only valid path (#8983) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 1 + .../examples/plot_map.cpp | 4 +- .../goal_planner_module.hpp | 13 +- .../pull_over_planner/freespace_pull_over.hpp | 2 +- .../pull_over_planner/geometric_pull_over.hpp | 2 +- .../pull_over_planner_base.hpp | 177 ++++++------------ .../pull_over_planner/shift_pull_over.hpp | 4 +- .../src/goal_planner_module.cpp | 139 +++++++------- .../pull_over_planner/freespace_pull_over.cpp | 13 +- .../pull_over_planner/geometric_pull_over.cpp | 13 +- .../pull_over_planner_base.cpp | 154 +++++++++++++++ .../src/pull_over_planner/shift_pull_over.cpp | 26 +-- .../src/util.cpp | 12 +- 13 files changed, 332 insertions(+), 228 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index 626b47b6bdb0d..d3f7f7a4015f0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -10,6 +10,7 @@ autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_path_planner plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED + src/pull_over_planner/pull_over_planner_base.cpp src/pull_over_planner/freespace_pull_over.cpp src/pull_over_planner/geometric_pull_over.cpp src/pull_over_planner/shift_pull_over.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp index a51b2fd337512..7e9ccea9ac0c2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp @@ -265,7 +265,7 @@ int main(int argc, char ** argv) auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( *node, goal_planner_parameter, lane_departure_checker); const auto pull_over_path_opt = - shift_pull_over_planner.plan(planner_data, reference_path, route_msg.goal_pose); + shift_pull_over_planner.plan(0, 0, planner_data, reference_path, route_msg.goal_pose); pybind11::scoped_interpreter guard{}; auto plt = matplotlibcpp17::pyplot::import(); @@ -282,7 +282,7 @@ int main(int argc, char ** argv) std::cout << pull_over_path_opt.has_value() << std::endl; if (pull_over_path_opt) { const auto & pull_over_path = pull_over_path_opt.value(); - const auto full_path = pull_over_path.getFullPath(); + const auto & full_path = pull_over_path.full_path; plot_path_with_lane_id(ax, full_path); } ax.set_aspect(Args("equal")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 6c96ca0eafbb6..4858319a26365 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -160,17 +160,16 @@ class ThreadSafeData return false; } - return pull_over_path_->isValidPath(); + return true; } - PullOverPlannerType getPullOverPlannerType() const + std::optional getPullOverPlannerType() const { const std::lock_guard lock(mutex_); if (!pull_over_path_) { - return PullOverPlannerType::NONE; + return std::nullopt; } - - return pull_over_path_->type; + return pull_over_path_->type(); }; void reset() @@ -206,7 +205,7 @@ class ThreadSafeData void set_pull_over_path_no_lock(const PullOverPath & path) { pull_over_path_ = std::make_shared(path); - if (path.type != PullOverPlannerType::NONE && path.type != PullOverPlannerType::FREESPACE) { + if (path.type() != PullOverPlannerType::FREESPACE) { lane_parking_pull_over_path_ = std::make_shared(path); } @@ -216,7 +215,7 @@ class ThreadSafeData void set_pull_over_path_no_lock(const std::shared_ptr & path) { pull_over_path_ = path; - if (path->type != PullOverPlannerType::NONE && path->type != PullOverPlannerType::FREESPACE) { + if (path->type() != PullOverPlannerType::FREESPACE) { lane_parking_pull_over_path_ = path; } last_path_update_time_ = clock_->now(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index e3dccb3fc57ec..6ea27a48c0361 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -42,7 +42,7 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 2d7872707dd28..2dfcfb3dc6e9e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -43,7 +43,7 @@ class GeometricPullOver : public PullOverPlannerBase Pose getCl() const { return planner_.getCl(); } std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; std::vector generatePullOverPaths( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index e9332041a8321..d952f8ddbd22d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -31,7 +31,6 @@ using tier4_planning_msgs::msg::PathWithLaneId; namespace autoware::behavior_path_planner { enum class PullOverPlannerType { - NONE = 0, SHIFT, ARC_FORWARD, ARC_BACKWARD, @@ -40,137 +39,77 @@ enum class PullOverPlannerType { struct PullOverPath { - PullOverPlannerType type{PullOverPlannerType::NONE}; - std::vector partial_paths{}; - size_t path_idx{0}; - // accelerate with constant acceleration to the target velocity - std::vector> pairs_terminal_velocity_and_accel{}; - Pose start_pose{}; - Pose end_pose{}; - std::vector debug_poses{}; - size_t goal_id{}; - size_t id{}; - bool decided_velocity{false}; - - /** - * @brief Set paths and start/end poses - * By setting partial_paths, full_path, parking_path and curvature are also set at the same time - * @param input_partial_paths partial paths - * @param input_start_pose start pose - * @param input_end_pose end pose - */ - void setPaths( - const std::vector input_partial_paths, const Pose & input_start_pose, - const Pose & input_end_pose) - { - partial_paths = input_partial_paths; - start_pose = input_start_pose; - end_pose = input_end_pose; +public: + static std::optional create( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, + const Pose & end_pose, + const std::vector> & pairs_terminal_velocity_and_accel); - updatePathData(); - } + PullOverPath(const PullOverPath & other); - // Note: return copy value (not const&) because the value is used in multi threads - PathWithLaneId getFullPath() const { return full_path; } + PullOverPlannerType type() const { return type_; } + size_t goal_id() const { return goal_id_; } + size_t id() const { return id_; } + Pose start_pose() const { return start_pose_; } + Pose end_pose() const { return end_pose_; } - PathWithLaneId getParkingPath() const { return parking_path; } + std::vector & partial_paths() { return partial_paths_; } + const std::vector & partial_paths() const { return partial_paths_; } - PathWithLaneId getCurrentPath() const - { - if (partial_paths.empty()) { - return PathWithLaneId{}; - } else if (partial_paths.size() <= path_idx) { - return partial_paths.back(); - } - return partial_paths.at(path_idx); - } + // TODO(soblin): use reference to avoid copy once thread-safe design is finished + PathWithLaneId full_path() const { return full_path_; } + PathWithLaneId parking_path() const { return parking_path_; } + std::vector full_path_curvatures() { return full_path_curvatures_; } + std::vector parking_path_curvatures() const { return parking_path_curvatures_; } + double full_path_max_curvature() const { return full_path_max_curvature_; } + double parking_path_max_curvature() const { return parking_path_max_curvature_; } + size_t path_idx() const { return path_idx_; } - bool incrementPathIndex() - { - if (partial_paths.size() - 1 <= path_idx) { - return false; - } - path_idx += 1; - return true; - } + bool incrementPathIndex(); - bool isValidPath() const { return type != PullOverPlannerType::NONE; } + // TODO(soblin): this cannot be const due to decelerateBeforeSearchStart + PathWithLaneId & getCurrentPath(); - std::vector getFullPathCurvatures() const { return full_path_curvatures; } - std::vector getParkingPathCurvatures() const { return parking_path_curvatures; } - double getFullPathMaxCurvature() const { return full_path_max_curvature; } - double getParkingPathMaxCurvature() const { return parking_path_max_curvature; } + const PathWithLaneId & getCurrentPath() const; -private: - void updatePathData() + std::pair getPairsTerminalVelocityAndAccel() const { - updateFullPath(); - updateParkingPath(); - updateCurvatures(); - } - - void updateFullPath() - { - PathWithLaneId path{}; - for (size_t i = 0; i < partial_paths.size(); ++i) { - if (i == 0) { - path.points.insert( - path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); - } else { - // skip overlapping point - path.points.insert( - path.points.end(), next(partial_paths.at(i).points.begin()), - partial_paths.at(i).points.end()); - } + if (pairs_terminal_velocity_and_accel_.size() <= path_idx_) { + return std::make_pair(0.0, 0.0); } - full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + return pairs_terminal_velocity_and_accel_.at(path_idx_); } - void updateParkingPath() - { - if (full_path.points.empty()) { - updateFullPath(); - } - const size_t start_idx = - autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); - - PathWithLaneId path{}; - std::copy( - full_path.points.begin() + start_idx, full_path.points.end(), - std::back_inserter(path.points)); - parking_path = path; - } + std::vector debug_poses{}; - void updateCurvatures() - { - const auto calculateCurvaturesAndMax = - [](const auto & path) -> std::pair, double> { - std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); - double max_curvature = 0.0; - if (!curvatures.empty()) { - max_curvature = std::abs(*std::max_element( - curvatures.begin(), curvatures.end(), - [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); - } - return std::make_pair(curvatures, max_curvature); - }; - std::tie(full_path_curvatures, full_path_max_curvature) = - calculateCurvaturesAndMax(getFullPath()); - std::tie(parking_path_curvatures, parking_path_max_curvature) = - calculateCurvaturesAndMax(getParkingPath()); - } +private: + PullOverPath( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const Pose & start_pose, const Pose & end_pose, + const std::vector & partial_paths, const PathWithLaneId & full_path, + const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel); + + PullOverPlannerType type_; + size_t goal_id_; + size_t id_; + Pose start_pose_; + Pose end_pose_; + + std::vector partial_paths_; + PathWithLaneId full_path_; + PathWithLaneId parking_path_; + std::vector full_path_curvatures_; + std::vector parking_path_curvatures_; + double full_path_max_curvature_; + double parking_path_max_curvature_; - // curvatures - std::vector full_path_curvatures{}; - std::vector parking_path_curvatures{}; - std::vector current_path_curvatures{}; - double parking_path_max_curvature{0.0}; - double full_path_max_curvature{0.0}; - double current_path_max_curvature{0.0}; - - // path - PathWithLaneId full_path{}; - PathWithLaneId parking_path{}; + // accelerate with constant acceleration to the target velocity + size_t path_idx_; + std::vector> pairs_terminal_velocity_and_accel_; }; class PullOverPlannerBase @@ -186,7 +125,7 @@ class PullOverPlannerBase virtual PullOverPlannerType getPlannerType() const = 0; virtual std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) = 0; protected: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index c5640d9b4949f..9baceb4430dec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -36,7 +36,7 @@ class ShiftPullOver : public PullOverPlannerBase const LaneDepartureChecker & lane_departure_checker); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) override; protected: @@ -46,7 +46,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional cropPrevModulePath( const PathWithLaneId & prev_module_path, const Pose & shift_end_pose) const; std::optional generatePullOverPath( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 4a59857725a5e..ee68800b81ea1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -20,7 +20,6 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" @@ -298,22 +297,20 @@ void GoalPlannerModule::onTimer() const auto planCandidatePaths = [&]( const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { - auto pull_over_path = - planner->plan(local_planner_data, previous_module_output, goal_candidate.goal_pose); - if (pull_over_path && pull_over_path->getParkingPath().points.size() >= 3) { - pull_over_path->goal_id = goal_candidate.id; - pull_over_path->id = path_candidates.size(); - + const auto pull_over_path = planner->plan( + goal_candidate.id, path_candidates.size(), local_planner_data, previous_module_output, + goal_candidate.goal_pose); + if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = - lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose).length; + lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose()).length; if (start_arc_length < min_start_arc_length) { min_start_arc_length = start_arc_length; // closest start pose is stop point when not finding safe path - closest_start_pose = pull_over_path->start_pose; + closest_start_pose = pull_over_path->start_pose(); } } }; @@ -797,10 +794,10 @@ bool GoalPlannerModule::planFreespacePath( if (!goal_candidate.is_safe) { continue; } - auto freespace_path = freespace_planner_->plan( - planner_data, BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK + const auto freespace_path = freespace_planner_->plan( + goal_candidate.id, 0, planner_data, + BehaviorModuleOutput{}, // NOTE: not used so passing {} is OK goal_candidate.goal_pose); - freespace_path->goal_id = goal_candidate.id; if (!freespace_path) { continue; } @@ -832,8 +829,8 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte return false; } - const PathWithLaneId path = lane_parking_path->getFullPath(); - const std::vector curvatures = lane_parking_path->getFullPathCurvatures(); + const PathWithLaneId path = lane_parking_path->full_path(); + const std::vector curvatures = lane_parking_path->full_path_curvatures(); if ( parameters_->use_object_recognition && goal_planner_utils::checkObjectsCollision( @@ -931,7 +928,7 @@ std::optional> GoalPlannerModule::selectP } for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { const auto & path = pull_over_path_candidates[i]; - const auto goal_candidate_it = goal_candidate_map.find(path.goal_id); + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { sorted_path_indices.push_back(i); } @@ -976,14 +973,14 @@ std::optional> GoalPlannerModule::selectP [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; - return goal_id_to_index[a.goal_id] < goal_id_to_index[b.goal_id]; + return goal_id_to_index[a.goal_id()] < goal_id_to_index[b.goal_id()]; }); // compare to sort pull_over_path_candidates based on the order in efficient_path_order const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { const auto & order = parameters_->efficient_path_order; - const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type)); - const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type)); + const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); + const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); return a_pos < b_pos; }; @@ -1005,13 +1002,13 @@ std::optional> GoalPlannerModule::selectP for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( - path.getParkingPath(), target_objects, planner_data_->parameters, false, "max"); + path.parking_path(), target_objects, planner_data_->parameters, false, "max"); auto it = std::lower_bound( margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); if (it == margins_with_zero.end()) { - path_id_to_rough_margin_map[path.id] = margins_with_zero.back(); + path_id_to_rough_margin_map[path.id()] = margins_with_zero.back(); } else { - path_id_to_rough_margin_map[path.id] = *it; + path_id_to_rough_margin_map[path.id()] = *it; } } @@ -1022,27 +1019,29 @@ std::optional> GoalPlannerModule::selectP const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; if ( - std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01) { + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01) { return false; } - return path_id_to_rough_margin_map[a.id] > path_id_to_rough_margin_map[b.id]; + return path_id_to_rough_margin_map[a.id()] > path_id_to_rough_margin_map[b.id()]; }); // STEP2-3: Sort by curvature // If the curvature is less than the threshold, prioritize the path. const auto isHighCurvature = [&](const PullOverPath & path) -> bool { - return path.getParkingPathMaxCurvature() >= parameters_->high_curvature_threshold; + return path.parking_path_max_curvature() >= parameters_->high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { - const double margin = path_id_to_rough_margin_map[path.id]; + const double margin = path_id_to_rough_margin_map[path.id()]; return std::any_of( soft_margins.begin(), soft_margins.end(), [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); }; const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { return !isSoftMargin(a) && !isSoftMargin(b) && - std::abs(path_id_to_rough_margin_map[a.id] - path_id_to_rough_margin_map[b.id]) < 0.01; + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01; }; // NOTE: this is just partition sort based on curvature threshold within each sub partitions @@ -1110,8 +1109,8 @@ std::optional> GoalPlannerModule::selectP parameters_->object_recognition_collision_check_hard_margins.back(); for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; - const PathWithLaneId parking_path = path.getParkingPath(); - const auto parking_path_curvatures = path.getParkingPathCurvatures(); + const PathWithLaneId & parking_path = path.parking_path(); + const auto & parking_path_curvatures = path.parking_path_curvatures(); if ( parameters_->use_object_recognition && goal_planner_utils::checkObjectsCollision( @@ -1127,7 +1126,7 @@ std::optional> GoalPlannerModule::selectP checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } - return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id))); + return std::make_pair(path, goal_candidates.at(goal_id_to_index.at(path.goal_id()))); } return {}; } @@ -1199,7 +1198,8 @@ void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt && planner_type_opt.value() == 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; @@ -1262,7 +1262,8 @@ void GoalPlannerModule::decideVelocity() const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths.front(); + // partial_paths + auto & first_path = thread_safe_data_.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) { @@ -1329,8 +1330,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( return getPreviousModuleOutput(); } + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); const bool is_freespace = - thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE; + planner_type_opt && planner_type_opt.value() == PullOverPlannerType::FREESPACE; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && @@ -1361,7 +1363,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( deceleratePath(pull_over_path); thread_safe_data_.set(goal_candidates, pull_over_path, modified_goal); RCLCPP_DEBUG( - getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id, + getLogger(), "selected pull over path: path_id: %ld, goal_id: %ld", pull_over_path.id(), modified_goal.id); } else { thread_safe_data_.set(goal_candidates); @@ -1390,7 +1392,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( } path_candidate_ = - std::make_shared(thread_safe_data_.get_pull_over_path()->getFullPath()); + std::make_shared(thread_safe_data_.get_pull_over_path()->full_path()); return output; } @@ -1422,12 +1424,12 @@ void GoalPlannerModule::postProcess() } updateSteeringFactor( - {thread_safe_data_.get_pull_over_path()->start_pose, + {thread_safe_data_.get_pull_over_path()->start_pose(), thread_safe_data_.get_modified_goal_pose()->goal_pose}, {distance_to_path_change.first, distance_to_path_change.second}, has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); - setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->getFullPath()); + setStopReason(StopReason::GOAL_PLANNER, thread_safe_data_.get_pull_over_path()->full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1458,7 +1460,7 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const return {std::numeric_limits::max(), std::numeric_limits::max()}; } - const auto full_path = thread_safe_data_.get_pull_over_path()->getFullPath(); + const auto full_path = thread_safe_data_.get_pull_over_path()->full_path(); const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1468,10 +1470,10 @@ std::pair GoalPlannerModule::calcDistanceToPathChange() const } const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - full_path.points, thread_safe_data_.get_pull_over_path()->start_pose.position); + full_path.points, thread_safe_data_.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, - thread_safe_data_.get_pull_over_path()->start_pose.position, start_pose_segment_idx); + thread_safe_data_.get_pull_over_path()->start_pose().position, start_pose_segment_idx); const size_t goal_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, thread_safe_data_.get_modified_goal_pose()->goal_pose.position); const double dist_to_parking_finish_pose = calcSignedArcLength( @@ -1540,7 +1542,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const std::invoke([&]() -> std::optional> { if (thread_safe_data_.foundPullOverPath()) { return std::make_pair( - thread_safe_data_.get_pull_over_path()->start_pose, "stop at selected start pose"); + thread_safe_data_.get_pull_over_path()->start_pose(), "stop at selected start pose"); } if (thread_safe_data_.get_closest_start_pose()) { return std::make_pair( @@ -1759,12 +1761,12 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const auto path = thread_safe_data_.get_pull_over_path()->getFullPath(); + const auto path = thread_safe_data_.get_pull_over_path()->full_path(); if (path.points.empty()) return getPreviousModuleOutput().turn_signal_info; const auto & current_pose = planner_data_->self_odometry->pose.pose; - const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose; - const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose; + const auto & start_pose = thread_safe_data_.get_pull_over_path()->start_pose(); + const auto & end_pose = thread_safe_data_.get_pull_over_path()->end_pose(); const auto shift_start_idx = autoware::motion_utils::findNearestIndex(path.points, start_pose.position); @@ -1808,12 +1810,13 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() constexpr bool is_lane_change = false; constexpr bool is_pull_over = true; const bool override_ego_stopped_check = std::invoke([&]() { - if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::SHIFT) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt && planner_type_opt.value() == PullOverPlannerType::SHIFT) { return false; } constexpr double distance_threshold = 1.0; const auto stop_point = - thread_safe_data_.get_pull_over_path()->partial_paths.front().points.back(); + thread_safe_data_.get_pull_over_path()->partial_paths().front().points.back(); const double distance_from_ego_to_stop_point = std::abs(autoware::motion_utils::calcSignedArcLength( path.points, stop_point.point.pose.position, current_pose.position)); @@ -1850,9 +1853,9 @@ bool GoalPlannerModule::hasEnoughDistance( // 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 = 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( - long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose.position); + long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); const double distance_to_restart = parameters_->decide_path_distance / 2; const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; @@ -1916,9 +1919,9 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const const auto closest_goal_candidate = goal_searcher_->getClosetGoalCandidateAlongLanes( thread_safe_data_.get_goal_candidates(), planner_data_); const auto decel_pose = calcLongitudinalOffsetPose( - pull_over_path.getFullPath().points, closest_goal_candidate.goal_pose.position, + pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position, -approximate_pull_over_distance_); - auto & first_path = pull_over_path.partial_paths.front(); + auto & first_path = pull_over_path.partial_paths().front(); if (decel_pose) { decelerateBeforeSearchStart(*decel_pose, first_path); return; @@ -2082,8 +2085,8 @@ bool GoalPlannerModule::isCrossingPossible( bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) const { const lanelet::ConstLanelets lanes = utils::transformToLanelets(generateDrivableLanes()); - const Pose & start_pose = pull_over_path.start_pose; - const Pose & end_pose = pull_over_path.end_pose; + const Pose & start_pose = pull_over_path.start_pose(); + const Pose & end_pose = pull_over_path.end_pose(); return isCrossingPossible(start_pose, end_pose, lanes); } @@ -2154,7 +2157,7 @@ bool GoalPlannerModule::isSafePath( return false; } const auto & pull_over_path = pull_over_path_opt.value(); - const auto current_pull_over_path = pull_over_path.getCurrentPath(); + const auto & current_pull_over_path = pull_over_path.getCurrentPath(); const auto & current_pose = planner_data->self_odometry->pose.pose; const double current_velocity = std::hypot( planner_data->self_odometry->twist.twist.linear.x, @@ -2169,8 +2172,7 @@ bool GoalPlannerModule::isSafePath( parameters.forward_goal_search_length); const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(current_pull_over_path.points); const std::pair terminal_velocity_and_accel = - utils::parking_departure::getPairsTerminalVelocityAndAccel( - pull_over_path.pairs_terminal_velocity_and_accel, pull_over_path.path_idx); + pull_over_path.getPairsTerminalVelocityAndAccel(); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); @@ -2347,18 +2349,18 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) // Visualize path and related pose if (thread_safe_data_.foundPullOverPath()) { add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->start_pose, "pull_over_start_pose", 0, 0.3, 0.3, + thread_safe_data_.get_pull_over_path()->start_pose(), "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - thread_safe_data_.get_pull_over_path()->end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); + thread_safe_data_.get_pull_over_path()->end_pose(), "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); add(createPathMarkerArray( - thread_safe_data_.get_pull_over_path()->getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); + thread_safe_data_.get_pull_over_path()->full_path(), "full_path", 0, 0.0, 0.5, 0.9)); add(createPathMarkerArray( thread_safe_data_.get_pull_over_path()->getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths.size(); ++i) { - const auto & partial_path = thread_safe_data_.get_pull_over_path()->partial_paths.at(i); + for (size_t i = 0; i < thread_safe_data_.get_pull_over_path()->partial_paths().size(); ++i) { + const auto & partial_path = thread_safe_data_.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)); } @@ -2462,11 +2464,12 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) marker.pose = thread_safe_data_.get_modified_goal_pose() ? thread_safe_data_.get_modified_goal_pose()->goal_pose : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); - if (thread_safe_data_.foundPullOverPath()) { + const auto planner_type_opt = thread_safe_data_.getPullOverPlannerType(); + if (planner_type_opt) { + marker.text = magic_enum::enum_name(planner_type_opt.value()); marker.text += - " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx) + "/" + - std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); + " " + std::to_string(thread_safe_data_.get_pull_over_path()->path_idx()) + "/" + + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths().size() - 1); } if (isStuck( @@ -2673,8 +2676,8 @@ PathDecisionState PathDecisionStateController::get_next_state( } // check current parking path collision - const auto & parking_path = pull_over_path.getParkingPath(); - const std::vector parking_path_curvatures = pull_over_path.getParkingPathCurvatures(); + const auto & parking_path = pull_over_path.parking_path(); + const std::vector parking_path_curvatures = pull_over_path.parking_path_curvatures(); const double margin = parameters.object_recognition_collision_check_hard_margins.back() * hysteresis_factor; if (goal_planner_utils::checkObjectsCollision( @@ -2710,10 +2713,10 @@ PathDecisionState PathDecisionStateController::get_next_state( autoware::motion_utils::findNearestSegmentIndex(current_path.points, current_pose.position); const size_t start_pose_segment_idx = autoware::motion_utils::findNearestSegmentIndex( - current_path.points, pull_over_path.start_pose.position); + current_path.points, pull_over_path.start_pose().position); const double dist_to_parking_start_pose = calcSignedArcLength( - current_path.points, current_pose.position, ego_segment_idx, pull_over_path.start_pose.position, - start_pose_segment_idx); + current_path.points, current_pose.position, ego_segment_idx, + pull_over_path.start_pose().position, start_pose_segment_idx); if (dist_to_parking_start_pose > parameters.decide_path_distance) { next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; return next_state; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 4b377aed46f7c..6127d66802dd8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -50,7 +50,7 @@ FreespacePullOver::FreespacePullOver( } std::optional FreespacePullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const Pose & current_pose = planner_data->self_odometry->pose.pose; @@ -138,10 +138,13 @@ std::optional FreespacePullOver::plan( } } - PullOverPath pull_over_path{}; - pull_over_path.pairs_terminal_velocity_and_accel = pairs_terminal_velocity_and_accel; - pull_over_path.setPaths(partial_paths, current_pose, goal_pose); - pull_over_path.type = getPlannerType(); + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, partial_paths, current_pose, goal_pose, + pairs_terminal_velocity_and_accel); + if (!pull_over_path_opt) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); return pull_over_path; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index eecdc11937463..438618a2282fe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -38,7 +38,7 @@ GeometricPullOver::GeometricPullOver( } std::optional GeometricPullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, [[maybe_unused]] const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const auto & route_handler = planner_data->route_handler; @@ -72,10 +72,13 @@ std::optional GeometricPullOver::plan( // check lane departure with road and shoulder lanes if (lane_departure_checker_.checkPathWillLeaveLane(lanes, arc_path)) return {}; - PullOverPath pull_over_path{}; - pull_over_path.type = getPlannerType(); - pull_over_path.pairs_terminal_velocity_and_accel = planner_.getPairsTerminalVelocityAndAccel(); - pull_over_path.setPaths(planner_.getPaths(), planner_.getStartPose(), planner_.getArcEndPose()); + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, planner_.getPaths(), planner_.getStartPose(), + planner_.getArcEndPose(), planner_.getPairsTerminalVelocityAndAccel()); + if (!pull_over_path_opt) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); return pull_over_path; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp new file mode 100644 index 0000000000000..f6535e7adb8f8 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -0,0 +1,154 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace autoware::behavior_path_planner +{ + +std::optional PullOverPath::create( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, + const std::vector & partial_paths, const Pose & start_pose, const Pose & end_pose, + const std::vector> & pairs_terminal_velocity_and_accel) +{ + if (partial_paths.empty()) { + return std::nullopt; + } + PathWithLaneId path{}; + for (size_t i = 0; i < partial_paths.size(); ++i) { + if (i == 0) { + path.points.insert( + path.points.end(), partial_paths.at(i).points.begin(), partial_paths.at(i).points.end()); + } else { + // skip overlapping point + path.points.insert( + path.points.end(), next(partial_paths.at(i).points.begin()), + partial_paths.at(i).points.end()); + } + } + PathWithLaneId full_path{}; + full_path.points = autoware::motion_utils::removeOverlapPoints(path.points); + if (full_path.points.size() < 3) { + return std::nullopt; + } + + const size_t start_idx = + autoware::motion_utils::findNearestIndex(full_path.points, start_pose.position); + + PathWithLaneId parking_path{}; + std::copy( + full_path.points.begin() + start_idx, full_path.points.end(), + std::back_inserter(parking_path.points)); + + if (parking_path.points.size() < 3) { + return std::nullopt; + } + + const auto calculateCurvaturesAndMax = + [](const auto & path) -> std::pair, double> { + std::vector curvatures = autoware::motion_utils::calcCurvature(path.points); + double max_curvature = 0.0; + if (!curvatures.empty()) { + max_curvature = std::abs(*std::max_element( + curvatures.begin(), curvatures.end(), + [](const double & a, const double & b) { return std::abs(a) < std::abs(b); })); + } + return std::make_pair(curvatures, max_curvature); + }; + + std::vector full_path_curvatures{}; + std::vector parking_path_curvatures{}; + double full_path_max_curvature{}; + double parking_path_max_curvature{}; + std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); + std::tie(parking_path_curvatures, parking_path_max_curvature) = + calculateCurvaturesAndMax(full_path); + + return PullOverPath( + type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, + full_path_curvatures, parking_path_curvatures, full_path_max_curvature, + parking_path_max_curvature, pairs_terminal_velocity_and_accel); +} + +PullOverPath::PullOverPath(const PullOverPath & other) +: type_(other.type_), + goal_id_(other.goal_id_), + id_(other.id_), + start_pose_(other.start_pose_), + end_pose_(other.end_pose_), + partial_paths_(other.partial_paths_), + full_path_(other.full_path_), + parking_path_(other.parking_path_), + full_path_curvatures_(other.full_path_curvatures_), + parking_path_curvatures_(other.parking_path_curvatures_), + full_path_max_curvature_(other.full_path_max_curvature_), + parking_path_max_curvature_(other.parking_path_max_curvature_), + path_idx_(other.path_idx_), + pairs_terminal_velocity_and_accel_(other.pairs_terminal_velocity_and_accel_) +{ +} + +PullOverPath::PullOverPath( + const PullOverPlannerType & type, const size_t goal_id, const size_t id, const Pose & start_pose, + const Pose & end_pose, const std::vector & partial_paths, + const PathWithLaneId & full_path, const PathWithLaneId & parking_path, + const std::vector & full_path_curvatures, + const std::vector & parking_path_curvatures, const double full_path_max_curvature, + const double parking_path_max_curvature, + const std::vector> & pairs_terminal_velocity_and_accel) +: type_(type), + goal_id_(goal_id), + id_(id), + start_pose_(start_pose), + end_pose_(end_pose), + partial_paths_(partial_paths), + full_path_(full_path), + parking_path_(parking_path), + full_path_curvatures_(full_path_curvatures), + parking_path_curvatures_(parking_path_curvatures), + full_path_max_curvature_(full_path_max_curvature), + parking_path_max_curvature_(parking_path_max_curvature), + path_idx_(0), + pairs_terminal_velocity_and_accel_(pairs_terminal_velocity_and_accel) +{ +} + +bool PullOverPath::incrementPathIndex() +{ + { + if (partial_paths_.size() - 1 <= path_idx_) { + return false; + } + path_idx_ += 1; + return true; + } +} + +PathWithLaneId & PullOverPath::getCurrentPath() +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +const PathWithLaneId & PullOverPath::getCurrentPath() const +{ + if (partial_paths_.size() <= path_idx_) { + return partial_paths_.back(); + } + return partial_paths_.at(path_idx_); +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 4536fc8873b40..645d74b6385da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -35,7 +35,7 @@ ShiftPullOver::ShiftPullOver( { } std::optional ShiftPullOver::plan( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const Pose & goal_pose) { const auto & route_handler = planner_data->route_handler; @@ -59,7 +59,8 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, lateral_jerk); + goal_id, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, goal_pose, + lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -126,7 +127,7 @@ std::optional ShiftPullOver::cropPrevModulePath( } std::optional ShiftPullOver::generatePullOverPath( - const std::shared_ptr planner_data, + const size_t goal_id, const size_t id, const std::shared_ptr planner_data, const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const @@ -254,13 +255,14 @@ std::optional ShiftPullOver::generatePullOverPath( } // set pull over path - PullOverPath pull_over_path{}; - pull_over_path.type = getPlannerType(); - std::vector partial_paths{shifted_path.path}; - pull_over_path.setPaths( - partial_paths, path_shifter.getShiftLines().front().start, - path_shifter.getShiftLines().front().end); - pull_over_path.pairs_terminal_velocity_and_accel.push_back(std::make_pair(pull_over_velocity, 0)); + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), goal_id, id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + path_shifter.getShiftLines().front().end, {std::make_pair(pull_over_velocity, 0)}); + + if (!pull_over_path_opt) { + return {}; + } + auto & pull_over_path = pull_over_path_opt.value(); pull_over_path.debug_poses.push_back(shift_end_pose_prev_module_path); pull_over_path.debug_poses.push_back(actual_shift_end_pose); pull_over_path.debug_poses.push_back(goal_pose); @@ -275,7 +277,7 @@ std::optional ShiftPullOver::generatePullOverPath( const auto parking_lot_polygons = lanelet::utils::query::getAllParkingLots(planner_data->route_handler->getLaneletMapPtr()); const auto path_footprints = goal_planner_utils::createPathFootPrints( - pull_over_path.getParkingPath(), p.base_link2front, p.base_link2rear, p.vehicle_width); + pull_over_path.parking_path(), p.base_link2front, p.base_link2rear, p.vehicle_width); const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { return std::any_of( parking_lot_polygons.begin(), parking_lot_polygons.end(), @@ -299,7 +301,7 @@ std::optional ShiftPullOver::generatePullOverPath( const auto combined_drivable = utils::combineDrivableLanes( expanded_lanes, previous_module_output.drivable_area_info.drivable_lanes); return !lane_departure_checker_.checkPathWillLeaveLane( - utils::transformToLanelets(combined_drivable), pull_over_path.getParkingPath()); + utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); }); if (!is_in_parking_lots && !is_in_lanes) { return {}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index beed916a31a1b..145c2ecb9c4f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -671,16 +671,16 @@ std::string makePathPriorityDebugMessage( for (size_t i = 0; i < sorted_path_indices.size(); ++i) { const auto & path = pull_over_path_candidates[sorted_path_indices[i]]; // goal_index is same to goal priority including unsafe goal - const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id())); const bool is_safe_goal = goal_candidates[goal_index].is_safe; - const int goal_priority = goal_id_and_priority[path.goal_id]; + const int goal_priority = goal_id_and_priority[path.goal_id()]; - ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) - << ", path_id: " << path.id << ", goal_id: " << path.goal_id + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type()) + << ", path_id: " << path.id() << ", goal_id: " << path.goal_id() << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") - << ", margin: " << path_id_to_rough_margin_map.at(path.id) + << ", margin: " << path_id_to_rough_margin_map.at(path.id()) << (isSoftMargin(path) ? " (soft)" : " (hard)") - << ", curvature: " << path.getParkingPathMaxCurvature() + << ", curvature: " << path.parking_path_max_curvature() << (isHighCurvature(path) ? " (high)" : " (low)") << "\n"; } ss << "-----------------------------------------------------------\n"; From 1d4e9498c69079ef28032df9716001345fc2a768 Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:07:02 +0900 Subject: [PATCH 12/27] refactor(autoware_pointcloud_preprocessor): rework ring outlier filter parameters (#8468) * feat: rework ring outlier parameters Signed-off-by: vividf * chore: add explicit cast Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: remove filter.param Signed-off-by: vividf * chore: set default frame Signed-off-by: vividf * chore: add maximum boundary Signed-off-by: vividf * chore: boundary to float type Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- .../ring_outlier_filter_node.param.yaml | 14 +++ .../docs/ring-outlier-filter.md | 15 +-- ...delet.hpp => ring_outlier_filter_node.hpp} | 8 +- .../ring_outlier_filter_node.launch.xml | 16 +++ .../ring_outlier_filter_node.schema.json | 113 ++++++++++++++++++ ...delet.cpp => ring_outlier_filter_node.cpp} | 33 +++-- 7 files changed, 165 insertions(+), 36 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/{ring_outlier_filter_nodelet.hpp => ring_outlier_filter_node.hpp} (97%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/outlier_filter/{ring_outlier_filter_nodelet.cpp => ring_outlier_filter_node.cpp} (93%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 02ce2a0098220..87f198e3f48ae 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -70,7 +70,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/downsample_filter/random_downsample_filter_node.cpp src/downsample_filter/approximate_downsample_filter_nodelet.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp - src/outlier_filter/ring_outlier_filter_nodelet.cpp + src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_node.cpp src/outlier_filter/dual_return_outlier_filter_nodelet.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..76bf68958f504 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/ring_outlier_filter_node.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + distance_ratio: 1.03 + object_length_threshold: 0.1 + num_points_threshold: 4 + max_rings_num: 128 + max_points_num_per_ring: 4000 + publish_outlier_pointcloud: false + min_azimuth_deg: 0.0 + max_azimuth_deg: 360.0 + max_distance: 12.0 + vertical_bins: 128 + horizontal_bins: 36 + noise_threshold: 2 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md index 7f2efe90a674f..95ea2ed8ba969 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -56,20 +56,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | -| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | -| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation | -| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation | -| `max_distance` | float | 12.0 | The limit distance for visibility score calculation | -| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram | -| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram | -| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json") }} | ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp similarity index 97% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index caedeac62b88a..cf396e3e3a4be 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" #include "autoware/pointcloud_preprocessor/transform_info.hpp" @@ -108,4 +108,4 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil }; } // namespace autoware::pointcloud_preprocessor -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODELET_HPP_ +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__RING_OUTLIER_FILTER_NODE_HPP_ diff --git a/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..68076f5c9c321 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/ring_outlier_filter_node.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..1c0cb406584ac --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/ring_outlier_filter_node.schema.json @@ -0,0 +1,113 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Ring Outlier Filter Node", + "type": "object", + "definitions": { + "ring_outlier_filter": { + "type": "object", + "properties": { + "distance_ratio": { + "type": "number", + "description": "distance_ratio", + "default": "1.03", + "minimum": 0.0 + }, + "object_length_threshold": { + "type": "number", + "description": "object_length_threshold", + "default": "0.1", + "minimum": 0.0 + }, + "num_points_threshold": { + "type": "integer", + "description": "num_points_threshold", + "default": "4", + "minimum": 0 + }, + "max_rings_num": { + "type": "integer", + "description": "max_rings_num", + "default": "128", + "minimum": 1 + }, + "max_points_num_per_ring": { + "type": "integer", + "description": "Set this value large enough such that HFoV / resolution < max_points_num_per_ring", + "default": "4000", + "minimum": 0 + }, + "publish_outlier_pointcloud": { + "type": "boolean", + "description": "Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments.", + "default": "false" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth for visibility score calculation", + "default": "0.0", + "minimum": 0.0 + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth for visibility score calculation", + "default": "360.0", + "minimum": 0.0, + "maximum": 360.0 + }, + "max_distance": { + "type": "number", + "description": "The limit distance for visibility score calculation", + "default": "12.0", + "minimum": 0.0 + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bin for visibility histogram", + "default": "128", + "minimum": 1 + }, + "horizontal_bins": { + "type": "integer", + "description": "The number of horizontal bin for visibility histogram", + "default": "36", + "minimum": 1 + }, + "noise_threshold": { + "type": "integer", + "description": "The threshold value for distinguishing noise from valid points in the frequency image", + "default": "2", + "minimum": 0 + } + }, + "required": [ + "distance_ratio", + "object_length_threshold", + "num_points_threshold", + "max_rings_num", + "max_points_num_per_ring", + "publish_outlier_pointcloud", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "horizontal_bins", + "noise_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/ring_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp similarity index 93% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index e71158ccf32d6..bf140e662440b 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -47,22 +47,21 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions // set initial parameters { - distance_ratio_ = static_cast(declare_parameter("distance_ratio", 1.03)); - object_length_threshold_ = - static_cast(declare_parameter("object_length_threshold", 0.1)); - num_points_threshold_ = static_cast(declare_parameter("num_points_threshold", 4)); - max_rings_num_ = static_cast(declare_parameter("max_rings_num", 128)); + distance_ratio_ = declare_parameter("distance_ratio"); + object_length_threshold_ = declare_parameter("object_length_threshold"); + num_points_threshold_ = declare_parameter("num_points_threshold"); + max_rings_num_ = static_cast(declare_parameter("max_rings_num")); max_points_num_per_ring_ = - static_cast(declare_parameter("max_points_num_per_ring", 4000)); - publish_outlier_pointcloud_ = - static_cast(declare_parameter("publish_outlier_pointcloud", false)); - - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 0.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 360.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - horizontal_bins_ = static_cast(declare_parameter("horizontal_bins", 36)); - noise_threshold_ = static_cast(declare_parameter("noise_threshold", 2)); + static_cast(declare_parameter("max_points_num_per_ring")); + + publish_outlier_pointcloud_ = declare_parameter("publish_outlier_pointcloud"); + + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + horizontal_bins_ = declare_parameter("horizontal_bins"); + noise_threshold_ = declare_parameter("noise_threshold"); } using std::placeholders::_1; From 35c4f594a923dd7ad1b02392f090515182ee126e Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 1 Oct 2024 12:45:50 +0900 Subject: [PATCH 13/27] refactor(goal_planner): remove unused header and divide ThreadSafeData to another file (#8990) Signed-off-by: Mamoru Sobue --- .../decision_state.hpp | 87 +++++++ .../goal_planner_module.hpp | 238 +----------------- .../goal_searcher.hpp | 1 - .../manager.hpp | 9 +- .../pull_over_planner/freespace_pull_over.hpp | 5 - .../thread_data.hpp | 202 +++++++++++++++ .../util.hpp | 2 - .../src/default_fixed_goal_planner.cpp | 2 - .../src/goal_planner_module.cpp | 5 + .../src/goal_searcher.cpp | 3 - .../src/manager.cpp | 10 +- .../pull_over_planner/freespace_pull_over.cpp | 11 +- .../pull_over_planner/geometric_pull_over.cpp | 4 +- .../src/util.cpp | 2 - 14 files changed, 316 insertions(+), 265 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp new file mode 100644 index 0000000000000..67aa41a5af7e5 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/decision_state.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ + +#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" +#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" + +#include +#include + +namespace autoware::behavior_path_planner +{ + +class PathDecisionState +{ +public: + enum class DecisionKind { + NOT_DECIDED, + DECIDING, + DECIDED, + }; + + DecisionKind state{DecisionKind::NOT_DECIDED}; + rclcpp::Time stamp{}; + bool is_stable_safe{false}; + std::optional safe_start_time{std::nullopt}; +}; + +class PathDecisionStateController +{ +public: + PathDecisionStateController() = default; + + /** + * @brief update current state and save old current state to prev state + */ + void transit_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const autoware_perception_msgs::msg::PredictedObjects & static_target_objects, + const autoware_perception_msgs::msg::PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path, + std::vector & ego_polygons_expanded); + + PathDecisionState get_current_state() const { return current_state_; } + + PathDecisionState get_prev_state() const { return prev_state_; } + +private: + PathDecisionState current_state_{}; + PathDecisionState prev_state_{}; + + /** + * @brief update current state and save old current state to prev state + */ + PathDecisionState get_next_state( + const bool found_pull_over_path, const rclcpp::Time & now, + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const std::optional modified_goal_opt, + const std::shared_ptr planner_data, + const std::shared_ptr occupancy_grid_map, + const bool is_current_safe, const GoalPlannerParameters & parameters, + const std::shared_ptr goal_searcher, const bool is_activated, + const std::optional & pull_over_path_opt, + std::vector & ego_polygons_expanded) const; +}; + +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__DECISION_STATE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 4858319a26365..d029b2f5953a7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -15,24 +15,15 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ -#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" -#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/thread_data.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "autoware/behavior_path_planner_common/utils/utils.hpp" -#include -#include #include -#include -#include #include #include @@ -41,7 +32,6 @@ #include #include -#include #include #include #include @@ -61,8 +51,6 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; using autoware::freespace_planning_algorithms::AstarParam; using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::PlannerCommonParam; -using autoware::freespace_planning_algorithms::RRTStar; -using autoware::freespace_planning_algorithms::RRTStarParam; using autoware::behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; @@ -71,186 +59,6 @@ using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckPa using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using autoware::universe_utils::Polygon2d; -#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) - -class PathDecisionState -{ -public: - enum class DecisionKind { - NOT_DECIDED, - DECIDING, - DECIDED, - }; - - DecisionKind state{DecisionKind::NOT_DECIDED}; - rclcpp::Time stamp{}; - bool is_stable_safe{false}; - std::optional safe_start_time{std::nullopt}; -}; - -class ThreadSafeData -{ -public: - ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) - : mutex_(mutex), clock_(clock) - { - } - - bool incrementPathIndex() - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - if (pull_over_path_->incrementPathIndex()) { - last_path_idx_increment_time_ = clock_->now(); - return true; - } - return false; - } - - void set_pull_over_path(const PullOverPath & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } - - void set_pull_over_path(const std::shared_ptr & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } - - template - void set(Args... args) - { - std::lock_guard lock(mutex_); - (..., set_no_lock(args)); - } - - void clearPullOverPath() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - } - - bool foundPullOverPath() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - return true; - } - - std::optional getPullOverPlannerType() const - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return std::nullopt; - } - return pull_over_path_->type(); - }; - - void reset() - { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - pull_over_path_candidates_.clear(); - goal_candidates_.clear(); - modified_goal_pose_ = std::nullopt; - last_path_update_time_ = std::nullopt; - last_path_idx_increment_time_ = std::nullopt; - closest_start_pose_ = std::nullopt; - last_previous_module_output_ = std::nullopt; - prev_data_ = PathDecisionState{}; - } - - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) - - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) - DEFINE_SETTER_GETTER_WITH_MUTEX(CollisionCheckDebugMap, collision_check) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) - DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) - -private: - void set_pull_over_path_no_lock(const PullOverPath & path) - { - pull_over_path_ = std::make_shared(path); - if (path.type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = std::make_shared(path); - } - - last_path_update_time_ = clock_->now(); - } - - void set_pull_over_path_no_lock(const std::shared_ptr & path) - { - pull_over_path_ = path; - if (path->type() != PullOverPlannerType::FREESPACE) { - lane_parking_pull_over_path_ = path; - } - last_path_update_time_ = clock_->now(); - } - - void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } - void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } - void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } - void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } - void set_no_lock(const CollisionCheckDebugMap & arg) { collision_check_ = arg; } - - std::shared_ptr pull_over_path_{nullptr}; - std::shared_ptr lane_parking_pull_over_path_{nullptr}; - std::vector pull_over_path_candidates_; - GoalCandidates goal_candidates_{}; - std::optional modified_goal_pose_; - std::optional last_path_update_time_; - std::optional last_path_idx_increment_time_; - std::optional closest_start_pose_{}; - std::optional last_previous_module_output_{}; - CollisionCheckDebugMap collision_check_{}; - PredictedObjects static_target_objects_{}; - PredictedObjects dynamic_target_objects_{}; - PathDecisionState prev_data_{}; - - std::recursive_mutex & mutex_; - rclcpp::Clock::SharedPtr clock_; -}; - -#undef DEFINE_SETTER_WITH_MUTEX -#undef DEFINE_GETTER_WITH_MUTEX -#undef DEFINE_SETTER_GETTER_WITH_MUTEX - struct FreespacePlannerDebugData { bool is_planning{false}; @@ -313,48 +121,6 @@ struct PullOverContextData const PredictedObjects dynamic_target_objects; }; -class PathDecisionStateController -{ -public: - PathDecisionStateController() = default; - - /** - * @brief update current state and save old current state to prev state - */ - void transit_state( - const bool found_pull_over_path, const rclcpp::Time & now, - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, - const std::optional & pull_over_path, - std::vector & ego_polygons_expanded); - - PathDecisionState get_current_state() const { return current_state_; } - - PathDecisionState get_prev_state() const { return prev_state_; } - -private: - PathDecisionState current_state_{}; - PathDecisionState prev_state_{}; - - /** - * @brief update current state and save old current state to prev state - */ - PathDecisionState get_next_state( - const bool found_pull_over_path, const rclcpp::Time & now, - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::optional modified_goal_opt, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const bool is_current_safe, const GoalPlannerParameters & parameters, - const std::shared_ptr goal_searcher, const bool is_activated, - const std::optional & pull_over_path_opt, - std::vector & ego_polygons_expanded) const; -}; - class GoalPlannerModule : public SceneModuleInterface { public: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index af2e756e64ca5..923fb6ae5bd44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "autoware/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp index 8ba6239630497..6e2aaedd98b0e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ -#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -38,12 +38,7 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface void init(rclcpp::Node * node) override; - std::unique_ptr createNewSceneModuleInstance() override - { - return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); - } + std::unique_ptr createNewSceneModuleInstance() override; void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index 6ea27a48c0361..34743ae5fbf5f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -18,19 +18,14 @@ #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include -#include -#include #include #include -#include namespace autoware::behavior_path_planner { using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; -using autoware::freespace_planning_algorithms::AstarSearch; -using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOver : public PullOverPlannerBase { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp new file mode 100644 index 0000000000000..3bfaa90b845fb --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -0,0 +1,202 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ + +#include "autoware/behavior_path_goal_planner_module/decision_state.hpp" +#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_path_planner +{ + +#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + void set_##NAME(const TYPE & value) \ + { \ + const std::lock_guard lock(mutex_); \ + NAME##_ = value; \ + } + +#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ +public: \ + TYPE get_##NAME() const \ + { \ + const std::lock_guard lock(mutex_); \ + return NAME##_; \ + } + +#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ + DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) + +class ThreadSafeData +{ +public: + ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) + : mutex_(mutex), clock_(clock) + { + } + + bool incrementPathIndex() + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + if (pull_over_path_->incrementPathIndex()) { + last_path_idx_increment_time_ = clock_->now(); + return true; + } + return false; + } + + void set_pull_over_path(const PullOverPath & path) + { + const std::lock_guard lock(mutex_); + set_pull_over_path_no_lock(path); + } + + void set_pull_over_path(const std::shared_ptr & path) + { + const std::lock_guard lock(mutex_); + set_pull_over_path_no_lock(path); + } + + template + void set(Args... args) + { + std::lock_guard lock(mutex_); + (..., set_no_lock(args)); + } + + void clearPullOverPath() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + } + + bool foundPullOverPath() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return false; + } + + return true; + } + + std::optional getPullOverPlannerType() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return std::nullopt; + } + return pull_over_path_->type(); + }; + + void reset() + { + const std::lock_guard lock(mutex_); + pull_over_path_ = nullptr; + pull_over_path_candidates_.clear(); + goal_candidates_.clear(); + modified_goal_pose_ = std::nullopt; + last_path_update_time_ = std::nullopt; + last_path_idx_increment_time_ = std::nullopt; + closest_start_pose_ = std::nullopt; + last_previous_module_output_ = std::nullopt; + prev_data_ = PathDecisionState{}; + } + + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, lane_parking_pull_over_path) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) + DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_idx_increment_time) + + DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(GoalCandidates, goal_candidates) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, modified_goal_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) + DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, last_previous_module_output) + DEFINE_SETTER_GETTER_WITH_MUTEX( + utils::path_safety_checker::CollisionCheckDebugMap, collision_check) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, static_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PredictedObjects, dynamic_target_objects) + DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) + +private: + void set_pull_over_path_no_lock(const PullOverPath & path) + { + pull_over_path_ = std::make_shared(path); + if (path.type() != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = std::make_shared(path); + } + + last_path_update_time_ = clock_->now(); + } + + void set_pull_over_path_no_lock(const std::shared_ptr & path) + { + pull_over_path_ = path; + if (path->type() != PullOverPlannerType::FREESPACE) { + lane_parking_pull_over_path_ = path; + } + last_path_update_time_ = clock_->now(); + } + + void set_no_lock(const GoalCandidates & arg) { goal_candidates_ = arg; } + void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } + void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } + void set_no_lock(const GoalCandidate & arg) { modified_goal_pose_ = arg; } + void set_no_lock(const BehaviorModuleOutput & arg) { last_previous_module_output_ = arg; } + void set_no_lock(const utils::path_safety_checker::CollisionCheckDebugMap & arg) + { + collision_check_ = arg; + } + + std::shared_ptr pull_over_path_{nullptr}; + std::shared_ptr lane_parking_pull_over_path_{nullptr}; + std::vector pull_over_path_candidates_; + GoalCandidates goal_candidates_{}; + std::optional modified_goal_pose_; + std::optional last_path_update_time_; + std::optional last_path_idx_increment_time_; + std::optional closest_start_pose_{}; + std::optional last_previous_module_output_{}; + utils::path_safety_checker::CollisionCheckDebugMap collision_check_{}; + PredictedObjects static_target_objects_{}; + PredictedObjects dynamic_target_objects_{}; + PathDecisionState prev_data_{}; + + std::recursive_mutex & mutex_; + rclcpp::Clock::SharedPtr clock_; +}; + +#undef DEFINE_SETTER_WITH_MUTEX +#undef DEFINE_GETTER_WITH_MUTEX +#undef DEFINE_SETTER_GETTER_WITH_MUTEX + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index f68800b47f5a5..52dfbbddc79ff 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -17,7 +17,6 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include @@ -31,7 +30,6 @@ #include #include -#include #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index 2709dbe8635d3..160bb33dc07de 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -14,8 +14,6 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index ee68800b81ea1..147fe9f79dba0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -14,8 +14,13 @@ #include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" #include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware/behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 8536001c6a08e..ea0816954d0e1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -15,13 +15,11 @@ #include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" #include "autoware_lanelet2_extension/utility/utilities.hpp" #include @@ -33,7 +31,6 @@ namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware::universe_utils::calcOffsetPose; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 32d6c55276876..34248a75eb1f5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -14,7 +14,8 @@ #include "autoware/behavior_path_goal_planner_module/manager.hpp" -#include "autoware/behavior_path_goal_planner_module/util.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include @@ -26,6 +27,13 @@ namespace autoware::behavior_path_planner { +std::unique_ptr GoalPlannerModuleManager::createNewSceneModuleInstance() +{ + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, steering_factor_interface_ptr_); +} + GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( rclcpp::Node * node, const std::string & base_ns) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index 6127d66802dd8..dbdac08c8778c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -20,11 +20,18 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include +#include + #include #include namespace autoware::behavior_path_planner { + +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; + FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) @@ -144,8 +151,6 @@ std::optional FreespacePullOver::plan( if (!pull_over_path_opt) { return {}; } - auto & pull_over_path = pull_over_path_opt.value(); - - return pull_over_path; + return pull_over_path_opt.value(); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index 438618a2282fe..6c4aee5b96abf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -78,8 +78,6 @@ std::optional GeometricPullOver::plan( if (!pull_over_path_opt) { return {}; } - auto & pull_over_path = pull_over_path_opt.value(); - - return pull_over_path; + return pull_over_path_opt.value(); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 145c2ecb9c4f7..3815acc4561e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -14,7 +14,6 @@ #include "autoware/behavior_path_goal_planner_module/util.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" @@ -34,7 +33,6 @@ #include #include -#include #include #include From bcfaed72a8335cc9040dbc7aa1361dbc5f16f81d Mon Sep 17 00:00:00 2001 From: Yukinari Hisaki <42021302+yhisaki@users.noreply.github.com> Date: Tue, 1 Oct 2024 12:46:50 +0900 Subject: [PATCH 14/27] feat(codecov): add codecov component for planning and control (#8992) * add planning-tier-iv-maintained-packages Signed-off-by: Y.Hisaki * add control component Signed-off-by: Y.Hisaki --------- Signed-off-by: Y.Hisaki --- codecov.yaml | 82 ++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) diff --git a/codecov.yaml b/codecov.yaml index c907c24818735..75fc6f8bacadd 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -31,3 +31,85 @@ ignore: - "**/test/*" - "**/test/**/*" - "**/debug.*" + +component_management: + individual_components: + - component_id: planning-tier-iv-maintained-packages + name: Planning TIER IV Maintained Packages + paths: + - planning/autoware_costmap_generator/** + - planning/autoware_external_velocity_limit_selector/** + - planning/autoware_freespace_planner/** + - planning/autoware_freespace_planning_algorithms/** + - planning/autoware_mission_planner/** + # - planning/autoware_objects_of_interest_marker_interface/** + - planning/autoware_obstacle_cruise_planner/** + # - planning/autoware_obstacle_stop_planner/** + - planning/autoware_path_optimizer/** + - planning/autoware_path_smoother/** + - planning/autoware_planning_test_manager/** + - planning/autoware_planning_topic_converter/** + - planning/autoware_planning_validator/** + - planning/autoware_remaining_distance_time_calculator/** + - planning/autoware_route_handler/** + - planning/autoware_rtc_interface/** + - planning/autoware_scenario_selector/** + - planning/autoware_static_centerline_generator/** + - planning/autoware_surround_obstacle_checker/** + - planning/autoware_velocity_smoother/** + ##### behavior_path_planner ##### + # - planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** + - planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_planner_common/** + - planning/behavior_path_planner/autoware_behavior_path_start_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/** + - planning/behavior_path_planner/autoware_behavior_path_lane_change_module/** + # - planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/** + - planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/** + # - planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** + - planning/behavior_path_planner/autoware_behavior_path_planner/** + - planning/behavior_path_planner/autoware_behavior_path_side_shift_module/** + ##### behavior_velocity_planner ##### + - planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_planner/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** + # - planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/** + - planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** + ##### motion_velocity_planner ##### + - planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** + + - component_id: control-tier-iv-maintained-packages + name: Control TIER IV Maintained Packages + paths: + - control/autoware_autonomous_emergency_braking/** + - control/autoware_control_validator/** + - control/autoware_external_cmd_selector/** + # - control/autoware_joy_controller/** + - control/autoware_lane_departure_checker/** + - control/autoware_mpc_lateral_controller/** + - control/autoware_operation_mode_transition_manager/** + - control/autoware_pid_longitudinal_controller/** + # - control/autoware_pure_pursuit/** + - control/autoware_shift_decider/** + # - control/autoware_smart_mpc_trajectory_follower/** + - control/autoware_trajectory_follower_base/** + - control/autoware_trajectory_follower_node/** + - control/autoware_vehicle_cmd_gate/** + # - control/control_performance_analysis/** + - control/obstacle_collision_checker/** + # - control/predicted_path_checker/** From 89d0a516502b980368cadd2739260e766dd2e129 Mon Sep 17 00:00:00 2001 From: Mitsuhiro Sakamoto <50359861+mitukou1109@users.noreply.github.com> Date: Tue, 1 Oct 2024 15:53:32 +0900 Subject: [PATCH 15/27] fix(universe_utils): avoid test timeout (#8993) reduce number of polygons to be generated Signed-off-by: mitukou1109 --- .../test/src/geometry/test_alt_geometry.cpp | 14 +++++++------- .../test/src/geometry/test_geometry.cpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp index b74c747fc8054..90dfb1ede4701 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_alt_geometry.cpp @@ -723,7 +723,7 @@ TEST(alt_geometry, within) TEST(alt_geometry, areaRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -763,7 +763,7 @@ TEST(alt_geometry, areaRand) TEST(alt_geometry, convexHullRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -811,7 +811,7 @@ TEST(alt_geometry, convexHullRand) TEST(alt_geometry, coveredByRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -878,7 +878,7 @@ TEST(alt_geometry, coveredByRand) TEST(alt_geometry, disjointRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -944,7 +944,7 @@ TEST(alt_geometry, disjointRand) TEST(alt_geometry, intersectsRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -1010,7 +1010,7 @@ TEST(alt_geometry, intersectsRand) TEST(alt_geometry, touchesRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -1077,7 +1077,7 @@ TEST(alt_geometry, touchesRand) TEST(alt_geometry, withinPolygonRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; diff --git a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp index ab5e9f4236bad..af90ab1f32c57 100644 --- a/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp +++ b/common/autoware_universe_utils/test/src/geometry/test_geometry.cpp @@ -1937,7 +1937,7 @@ TEST( TEST(geometry, intersectPolygonRand) { std::vector polygons; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; @@ -2227,7 +2227,7 @@ TEST(geometry, intersectConcavePolygonRand) { std::vector polygons; std::vector> triangulations; - constexpr auto polygons_nb = 500; + constexpr auto polygons_nb = 100; constexpr auto max_vertices = 10; constexpr auto max_values = 1000; From a3015399022abbad54965578d55c39d3a968aaf0 Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Tue, 1 Oct 2024 18:19:49 +0900 Subject: [PATCH 16/27] feat(autonomous_emergency_braking): add sanity chackes (#8998) add sanity chackes Signed-off-by: Daniel Sanchez --- .../src/node.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index ec53a677cdd81..34a7453b51726 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -461,7 +461,9 @@ bool AEB::checkCollision(MarkerArray & debug_markers) const auto ego_polys = generatePathFootprint(path, expand_width_); std::vector objects; // Crop out Pointcloud using an extra wide ego path - if (use_pointcloud_data_ && !points_belonging_to_cluster_hulls->empty()) { + if ( + use_pointcloud_data_ && points_belonging_to_cluster_hulls && + !points_belonging_to_cluster_hulls->empty()) { const auto current_time = obstacle_ros_pointcloud_ptr_->header.stamp; getClosestObjectsOnPath(path, current_time, points_belonging_to_cluster_hulls, objects); } @@ -712,6 +714,9 @@ void AEB::generatePathFootprint( const Path & path, const double extra_width_margin, std::vector & polygons) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return; + } for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -721,8 +726,11 @@ void AEB::generatePathFootprint( std::vector AEB::generatePathFootprint( const Path & path, const double extra_width_margin) { - std::vector polygons; autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (path.empty()) { + return {}; + } + std::vector polygons; for (size_t i = 0; i < path.size() - 1; ++i) { polygons.push_back( createPolygon(path.at(i), path.at(i + 1), vehicle_info_, extra_width_margin)); @@ -735,7 +743,7 @@ void AEB::createObjectDataUsingPredictedObjects( std::vector & object_data_vector) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if (predicted_objects_ptr_->objects.empty()) return; + if (predicted_objects_ptr_->objects.empty() || ego_polys.empty()) return; const double current_ego_speed = current_velocity_ptr_->longitudinal_velocity; const auto & objects = predicted_objects_ptr_->objects; @@ -926,6 +934,9 @@ void AEB::cropPointCloudWithEgoFootprintPath( const std::vector & ego_polys, PointCloud::Ptr filtered_objects) { autoware::universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + if (ego_polys.empty()) { + return; + } PointCloud::Ptr full_points_ptr(new PointCloud); pcl::fromROSMsg(*obstacle_ros_pointcloud_ptr_, *full_points_ptr); // Create a Point cloud with the points of the ego footprint From a2446ca664ae82000db563e0fbf417b8270c7bcb Mon Sep 17 00:00:00 2001 From: Yuxuan Liu <619684051@qq.com> Date: Tue, 1 Oct 2024 18:28:15 +0900 Subject: [PATCH 17/27] fix(docs): fix file name for bluetooth monitor schema (#8308) * fix file name for schema Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> * the variable name should be addresses instead Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --------- Signed-off-by: YuxuanLiuTier4Desktop <619684051@qq.com> --- ...ooth_monitor.shcema.json => bluetooth_monitor.schema.json} | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) rename system/bluetooth_monitor/schema/{bluetooth_monitor.shcema.json => bluetooth_monitor.schema.json} (93%) diff --git a/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json b/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json similarity index 93% rename from system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json rename to system/bluetooth_monitor/schema/bluetooth_monitor.schema.json index 6951ecd6aed88..0914c7ec9a9b9 100644 --- a/system/bluetooth_monitor/schema/bluetooth_monitor.shcema.json +++ b/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json @@ -6,7 +6,7 @@ "bluetooth_monitor": { "type": "object", "properties": { - "address": { + "addresses": { "type": "array", "description": "Bluetooth addresses of the device to monitor", "items": { @@ -30,7 +30,7 @@ "default": 0.1 } }, - "required": ["address", "port", "timeout", "rtt_warn"], + "required": ["addresses", "port", "timeout", "rtt_warn"], "additionalProperties": false } }, From 6a246835bbe5992d23b49a92e50421168549bb76 Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Tue, 1 Oct 2024 18:58:12 +0900 Subject: [PATCH 18/27] fix(autoware_lidar_centerpoint): fix twist covariance orientation (#8996) * fix(autoware_lidar_centerpoint): fix covariance converter considering the twist covariance matrix is based on the object coordinate Signed-off-by: Taekjin LEE fix style * fix: update test of box3DToDetectedObject function Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE --- .../autoware/lidar_centerpoint/ros_utils.hpp | 2 +- .../lib/ros_utils.cpp | 21 ++++++++++++++----- .../test/test_ros_utils.cpp | 7 ++++--- 3 files changed, 21 insertions(+), 9 deletions(-) diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp index de901b06e83c3..9a17cad91efb8 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/ros_utils.hpp @@ -35,7 +35,7 @@ uint8_t getSemanticType(const std::string & class_name); std::array convertPoseCovarianceMatrix(const Box3D & box3d); -std::array convertTwistCovarianceMatrix(const Box3D & box3d); +std::array convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw); bool isCarLikeVehicleLabel(const uint8_t label); diff --git a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp index fc383a8d1da00..feeab969e88bd 100644 --- a/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/lib/ros_utils.cpp @@ -50,7 +50,7 @@ void box3DToDetectedObject( // pose and shape // mmdet3d yaw format to ros yaw format - float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; + const float yaw = -box3d.yaw - autoware::universe_utils::pi / 2; obj.kinematics.pose_with_covariance.pose.position = autoware::universe_utils::createPoint(box3d.x, box3d.y, box3d.z); obj.kinematics.pose_with_covariance.pose.orientation = @@ -67,6 +67,8 @@ void box3DToDetectedObject( if (has_twist) { float vel_x = box3d.vel_x; float vel_y = box3d.vel_y; + + // twist of the object is based on the object coordinate system geometry_msgs::msg::Twist twist; twist.linear.x = std::cos(yaw) * vel_x + std::sin(yaw) * vel_y; twist.linear.y = -std::sin(yaw) * vel_x + std::cos(yaw) * vel_y; @@ -76,7 +78,7 @@ void box3DToDetectedObject( obj.kinematics.has_twist = has_twist; if (has_variance) { obj.kinematics.has_twist_covariance = has_variance; - obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d); + obj.kinematics.twist_with_covariance.covariance = convertTwistCovarianceMatrix(box3d, yaw); } } } @@ -113,12 +115,21 @@ std::array convertPoseCovarianceMatrix(const Box3D & box3d) return pose_covariance; } -std::array convertTwistCovarianceMatrix(const Box3D & box3d) +std::array convertTwistCovarianceMatrix(const Box3D & box3d, const float yaw) { using POSE_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // twist covariance matrix is based on the object coordinate system std::array twist_covariance{}; - twist_covariance[POSE_IDX::X_X] = box3d.vel_x_variance; - twist_covariance[POSE_IDX::Y_Y] = box3d.vel_y_variance; + const float cos_yaw = std::cos(yaw); + const float sin_yaw = std::sin(yaw); + twist_covariance[POSE_IDX::X_X] = + box3d.vel_x_variance * cos_yaw * cos_yaw + box3d.vel_y_variance * sin_yaw * sin_yaw; + twist_covariance[POSE_IDX::X_Y] = + (box3d.vel_y_variance - box3d.vel_x_variance) * sin_yaw * cos_yaw; + twist_covariance[POSE_IDX::Y_X] = twist_covariance[POSE_IDX::X_Y]; + twist_covariance[POSE_IDX::Y_Y] = + box3d.vel_x_variance * sin_yaw * sin_yaw + box3d.vel_y_variance * cos_yaw * cos_yaw; return twist_covariance; } diff --git a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp index 555d820ec3644..5605d2df6a9d9 100644 --- a/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp +++ b/perception/autoware_lidar_centerpoint/test/test_ros_utils.cpp @@ -125,13 +125,14 @@ TEST(TestSuite, convertPoseCovarianceMatrix) TEST(TestSuite, convertTwistCovarianceMatrix) { autoware::lidar_centerpoint::Box3D box3d; - box3d.vel_x_variance = 0.1; + box3d.vel_x_variance = 0.5; box3d.vel_y_variance = 0.2; + float yaw = 0; std::array twist_covariance = - autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d); + autoware::lidar_centerpoint::convertTwistCovarianceMatrix(box3d, yaw); - EXPECT_FLOAT_EQ(twist_covariance[0], 0.1); + EXPECT_FLOAT_EQ(twist_covariance[0], 0.5); EXPECT_FLOAT_EQ(twist_covariance[7], 0.2); } From 6fccdf5da390df6e67ff9fc1117f81e30934f79f Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Tue, 1 Oct 2024 20:25:44 +0900 Subject: [PATCH 19/27] refactor(autoware_pointcloud_preprocessor): rework dual return outlier filter parameters (#8475) * feat: rework dual return outlier filter parameters Signed-off-by: vividf * chore: fix readme Signed-off-by: vividf * chore: change launch file name Signed-off-by: vividf * chore: fix type Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: change boundary Signed-off-by: vividf * chore: fix boundary Signed-off-by: vividf * chore: fix json schema Signed-off-by: vividf * Update sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json Co-authored-by: Kenzo Lobos Tsunekawa * chore: fix grammar error Signed-off-by: vividf * chore: fix description for weak_first_local_noise_threshold Signed-off-by: vividf * chore: change minimum and maximum to float Signed-off-by: vividf --------- Signed-off-by: vividf Co-authored-by: Kenzo Lobos Tsunekawa --- .../CMakeLists.txt | 2 +- ...dual_return_outlier_filter_node.param.yaml | 19 +++ .../docs/dual-return-outlier-filter.md | 20 +-- ...pp => dual_return_outlier_filter_node.hpp} | 8 +- .../dual_return_outlier_filter.launch.xml | 21 --- ...dual_return_outlier_filter_node.launch.xml | 17 ++ ...ual_return_outlier_filter_node.schema.json | 146 ++++++++++++++++++ ...pp => dual_return_outlier_filter_node.cpp} | 45 +++--- 8 files changed, 208 insertions(+), 70 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/{dual_return_outlier_filter_nodelet.hpp => dual_return_outlier_filter_node.hpp} (95%) delete mode 100644 sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/outlier_filter/{dual_return_outlier_filter_nodelet.cpp => dual_return_outlier_filter_node.cpp} (90%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 87f198e3f48ae..2c93731972dc8 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -73,7 +73,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp src/outlier_filter/voxel_grid_outlier_filter_node.cpp - src/outlier_filter/dual_return_outlier_filter_nodelet.cpp + src/outlier_filter/dual_return_outlier_filter_node.cpp src/passthrough_filter/passthrough_filter_node.cpp src/passthrough_filter/passthrough_filter_uint16_node.cpp src/passthrough_filter/passthrough_uint16.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml new file mode 100644 index 0000000000000..5454176d7f319 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/dual_return_outlier_filter_node.param.yaml @@ -0,0 +1,19 @@ +/**: + ros__parameters: + x_max: 18.0 + x_min: -12.0 + y_max: 2.0 + y_min: -2.0 + z_max: 10.0 + z_min: 0.0 + min_azimuth_deg: 135.0 + max_azimuth_deg: 225.0 + max_distance: 12.0 + vertical_bins: 128 + max_azimuth_diff: 50.0 + weak_first_distance_ratio: 1.004 + general_distance_ratio: 1.03 + weak_first_local_noise_threshold: 2 + roi_mode: "Fixed_xyz_ROI" + visibility_error_threshold: 0.5 + visibility_warn_threshold: 0.7 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 6c9a7ed14c2eb..8f4da273861a3 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -50,25 +50,7 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Core Parameters -| Name | Type | Description | -| ---------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------- | -| `vertical_bins` | int | The number of vertical bin for visibility histogram | -| `max_azimuth_diff` | float | Threshold for ring_outlier_filter | -| `weak_first_distance_ratio` | double | Threshold for ring_outlier_filter | -| `general_distance_ratio` | double | Threshold for ring_outlier_filter | -| `weak_first_local_noise_threshold` | int | The parameter for determining whether it is noise | -| `visibility_error_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR | -| `visibility_warn_threshold` | float | When the percentage of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN | -| `roi_mode` | string | The name of ROI mode for switching | -| `min_azimuth_deg` | float | The left limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_azimuth_deg` | float | The right limit of azimuth for `Fixed_azimuth_ROI` mode | -| `max_distance` | float | The limit distance for for `Fixed_azimuth_ROI` mode | -| `x_max` | float | Maximum of x for `Fixed_xyz_ROI` mode | -| `x_min` | float | Minimum of x for `Fixed_xyz_ROI` mode | -| `y_max` | float | Maximum of y for `Fixed_xyz_ROI` mode | -| `y_min` | float | Minimum of y for `Fixed_xyz_ROI` mode | -| `z_max` | float | Maximum of z for `Fixed_xyz_ROI` mode | -| `z_min` | float | Minimum of z for `Fixed_xyz_ROI` mode | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp similarity index 95% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index b8aba769b17a5..ef33e88ef5c98 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -94,5 +94,5 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso } // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODELET_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__OUTLIER_FILTER__DUAL_RETURN_OUTLIER_FILTER_NODE_HPP_ // NOLINT // clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml deleted file mode 100644 index c65996fbcdc65..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter.launch.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml new file mode 100644 index 0000000000000..d0293ca140e4f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/dual_return_outlier_filter_node.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json new file mode 100644 index 0000000000000..baaa0fa1f4604 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/dual_return_outlier_filter_node.schema.json @@ -0,0 +1,146 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Dual Return Outlier Filter Node", + "type": "object", + "definitions": { + "dual_return_outlier_filter": { + "type": "object", + "properties": { + "x_max": { + "type": "number", + "description": "Maximum of x in meters for `Fixed_xyz_ROI` mode", + "default": "18.0" + }, + "x_min": { + "type": "number", + "description": "Minimum of x in meters for `Fixed_xyz_ROI` mode", + "default": "-12.0" + }, + "y_max": { + "type": "number", + "description": "Maximum of y in meters for `Fixed_xyz_ROI` mode", + "default": "2.0" + }, + "y_min": { + "type": "number", + "description": "Minimum of y in meters for `Fixed_xyz_ROI` mode", + "default": "-2.0" + }, + "z_max": { + "type": "number", + "description": "Maximum of z in meters for `Fixed_xyz_ROI` mode", + "default": "10.0" + }, + "z_min": { + "type": "number", + "description": "Minimum of z in meters for `Fixed_xyz_ROI` mode", + "default": "0.0" + }, + "min_azimuth_deg": { + "type": "number", + "description": "The left limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "135.0", + "minimum": 0, + "maximum": 360 + }, + "max_azimuth_deg": { + "type": "number", + "description": "The right limit of azimuth in degrees for `Fixed_azimuth_ROI` mode", + "default": "225.0", + "minimum": 0, + "maximum": 360 + }, + "max_distance": { + "type": "number", + "description": "The limit distance in meters for `Fixed_azimuth_ROI` mode", + "default": "12.0", + "minimum": 0.0 + }, + "vertical_bins": { + "type": "integer", + "description": "The number of vertical bins for the visibility histogram", + "default": "128", + "minimum": 1 + }, + "max_azimuth_diff": { + "type": "number", + "description": "The azimuth difference threshold in degrees for ring_outlier_filter", + "default": "50.0", + "minimum": 0.0 + }, + "weak_first_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive weak point distances", + "default": "1.004", + "minimum": 0.0 + }, + "general_distance_ratio": { + "type": "number", + "description": "The maximum allowed ratio between consecutive normal point distances", + "default": "1.03", + "minimum": 0.0 + }, + "weak_first_local_noise_threshold": { + "type": "integer", + "description": "If the number of outliers among weak points is less than the weak_first_local_noise_threshold in the (max_azimuth - min_azimuth) / horizontal_bins interval, all points within the interval will not be filtered out.", + "default": "2", + "minimum": 0 + }, + "roi_mode": { + "type": "string", + "description": "roi mode", + "default": "Fixed_xyz_ROI", + "enum": ["Fixed_xyz_ROI", "No_ROI", "Fixed_azimuth_ROI"] + }, + "visibility_error_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes ERR", + "default": "0.5", + "minimum": 0.0, + "maximum": 1.0 + }, + "visibility_warn_threshold": { + "type": "number", + "description": "When the proportion of white pixels in the binary histogram falls below this parameter the diagnostic status becomes WARN", + "default": "0.7", + "minimum": 0.0, + "maximum": 1.0 + } + }, + "required": [ + "x_max", + "x_min", + "y_max", + "y_min", + "z_max", + "z_min", + "min_azimuth_deg", + "max_azimuth_deg", + "max_distance", + "vertical_bins", + "max_azimuth_diff", + "weak_first_distance_ratio", + "general_distance_ratio", + "weak_first_local_noise_threshold", + "roi_mode", + "visibility_error_threshold", + "visibility_warn_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/dual_return_outlier_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp similarity index 90% rename from sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index 5fd3262088d83..d3f81473ed06f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2024 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp" #include "autoware_point_types/types.hpp" @@ -38,29 +38,24 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( { // set initial parameters { - x_max_ = static_cast(declare_parameter("x_max", 18.0)); - x_min_ = static_cast(declare_parameter("x_min", -12.0)); - y_max_ = static_cast(declare_parameter("y_max", 2.0)); - y_min_ = static_cast(declare_parameter("y_min", -2.0)); - z_max_ = static_cast(declare_parameter("z_max", 10.0)); - z_min_ = static_cast(declare_parameter("z_min", 0.0)); - min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 135.0)); - max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 225.0)); - max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); - vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); - max_azimuth_diff_ = static_cast(declare_parameter("max_azimuth_diff", 50.0)); - weak_first_distance_ratio_ = - static_cast(declare_parameter("weak_first_distance_ratio", 1.004)); - general_distance_ratio_ = - static_cast(declare_parameter("general_distance_ratio", 1.03)); - - weak_first_local_noise_threshold_ = - static_cast(declare_parameter("weak_first_local_noise_threshold", 2)); - roi_mode_ = static_cast(declare_parameter("roi_mode", "Fixed_xyz_ROI")); - visibility_error_threshold_ = - static_cast(declare_parameter("visibility_error_threshold", 0.5)); - visibility_warn_threshold_ = - static_cast(declare_parameter("visibility_warn_threshold", 0.7)); + x_max_ = declare_parameter("x_max"); + x_min_ = declare_parameter("x_min"); + y_max_ = declare_parameter("y_max"); + y_min_ = declare_parameter("y_min"); + z_max_ = declare_parameter("z_max"); + z_min_ = declare_parameter("z_min"); + min_azimuth_deg_ = declare_parameter("min_azimuth_deg"); + max_azimuth_deg_ = declare_parameter("max_azimuth_deg"); + max_distance_ = declare_parameter("max_distance"); + vertical_bins_ = declare_parameter("vertical_bins"); + max_azimuth_diff_ = declare_parameter("max_azimuth_diff"); + weak_first_distance_ratio_ = declare_parameter("weak_first_distance_ratio"); + general_distance_ratio_ = declare_parameter("general_distance_ratio"); + + weak_first_local_noise_threshold_ = declare_parameter("weak_first_local_noise_threshold"); + roi_mode_ = declare_parameter("roi_mode"); + visibility_error_threshold_ = declare_parameter("visibility_error_threshold"); + visibility_warn_threshold_ = declare_parameter("visibility_warn_threshold"); } updater_.setHardwareID("dual_return_outlier_filter"); updater_.add( From 6e2cffb3909fc9854298d40b39f1fe007f383f2d Mon Sep 17 00:00:00 2001 From: "Yi-Hsiang Fang (Vivid)" <146902905+vividf@users.noreply.github.com> Date: Wed, 2 Oct 2024 11:09:53 +0900 Subject: [PATCH 20/27] refactor(autoware_pointcloud_preprocessor): rework approximate downsample filter parameters (#8480) * feat: rework approximate downsample parameters Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: change double to float Signed-off-by: vividf * feat: rework approximate downsample parameters Signed-off-by: vividf * chore: add boundary Signed-off-by: vividf * chore: change double to float Signed-off-by: vividf * chore: fix grammatical error Signed-off-by: vividf * chore: fix variables from double to float in header Signed-off-by: vividf * chore: change minimum to float Signed-off-by: vividf * chore: fix CMakeLists Signed-off-by: vividf --------- Signed-off-by: vividf --- .../CMakeLists.txt | 2 +- ...roximate_downsample_filter_node.param.yaml | 5 ++ .../docs/downsample-filter.md | 6 +-- ...=> approximate_downsample_filter_node.hpp} | 14 +++--- .../approximate_downsample_filter.launch.xml | 16 +++++++ ...oximate_downsample_filter_node.schema.json | 46 +++++++++++++++++++ ...=> approximate_downsample_filter_node.cpp} | 10 ++-- 7 files changed, 81 insertions(+), 18 deletions(-) create mode 100644 sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml rename sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/{approximate_downsample_filter_nodelet.hpp => approximate_downsample_filter_node.hpp} (92%) create mode 100644 sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml create mode 100644 sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json rename sensing/autoware_pointcloud_preprocessor/src/downsample_filter/{approximate_downsample_filter_nodelet.cpp => approximate_downsample_filter_node.cpp} (93%) diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 2c93731972dc8..94deffd5ae9d0 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -68,7 +68,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED src/crop_box_filter/crop_box_filter_nodelet.cpp src/downsample_filter/voxel_grid_downsample_filter_node.cpp src/downsample_filter/random_downsample_filter_node.cpp - src/downsample_filter/approximate_downsample_filter_nodelet.cpp + src/downsample_filter/approximate_downsample_filter_node.cpp src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp src/outlier_filter/ring_outlier_filter_node.cpp src/outlier_filter/radius_search_2d_outlier_filter_node.cpp diff --git a/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml new file mode 100644 index 0000000000000..239e47f09632b --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/approximate_downsample_filter_node.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + voxel_size_x: 0.3 + voxel_size_y: 0.3 + voxel_size_z: 0.1 diff --git a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index c6a45f7212c46..92ca1d3b3081c 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -36,11 +36,7 @@ These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, #### Approximate Downsample Filter -| Name | Type | Default Value | Description | -| -------------- | ------ | ------------- | ---------------- | -| `voxel_size_x` | double | 0.3 | voxel size x [m] | -| `voxel_size_y` | double | 0.3 | voxel size y [m] | -| `voxel_size_z` | double | 0.1 | voxel size z [m] | +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json") }} ### Random Downsample Filter diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp similarity index 92% rename from sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp rename to sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp index 7f5d3f83d3a30..abef5acad59dc 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2024 TIER IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -48,8 +48,8 @@ * $Id: voxel_grid.cpp 35876 2011-02-09 01:04:36Z rusu $ * */ -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT +#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT #include "autoware/pointcloud_preprocessor/filter.hpp" @@ -72,9 +72,9 @@ class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preproc rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); private: - double voxel_size_x_; - double voxel_size_y_; - double voxel_size_z_; + float voxel_size_x_; + float voxel_size_y_; + float voxel_size_z_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW @@ -83,5 +83,5 @@ class ApproximateDownsampleFilterComponent : public autoware::pointcloud_preproc } // namespace autoware::pointcloud_preprocessor // clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODELET_HPP_ // NOLINT +#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__APPROXIMATE_DOWNSAMPLE_FILTER_NODE_HPP_ // NOLINT // clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml new file mode 100644 index 0000000000000..f4375e5a5cd2f --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/approximate_downsample_filter.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json new file mode 100644 index 0000000000000..7b582aa5377b4 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/approximate_downsample_filter_node.schema.json @@ -0,0 +1,46 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Approximate Downsample Filter Node", + "type": "object", + "definitions": { + "approximate_downsample_filter": { + "type": "object", + "properties": { + "voxel_size_x": { + "type": "number", + "description": "voxel size along the x-axis [m]", + "default": "0.3", + "minimum": 0.0 + }, + "voxel_size_y": { + "type": "number", + "description": "voxel size along the y-axis [m]", + "default": "0.3", + "minimum": 0.0 + }, + "voxel_size_z": { + "type": "number", + "description": "voxel size along the z-axis [m]", + "default": "0.1", + "minimum": 0.0 + } + }, + "required": ["voxel_size_x", "voxel_size_y", "voxel_size_z"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/approximate_downsample_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp similarity index 93% rename from sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp rename to sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp index a4ccca9beea72..8ae814cf0ef1d 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_nodelet.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/approximate_downsample_filter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. All rights reserved. +// Copyright 2024 TIER IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -49,7 +49,7 @@ * */ -#include "autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_nodelet.hpp" +#include "autoware/pointcloud_preprocessor/downsample_filter/approximate_downsample_filter_node.hpp" #include #include @@ -64,9 +64,9 @@ ApproximateDownsampleFilterComponent::ApproximateDownsampleFilterComponent( : Filter("ApproximateDownsampleFilter", options) { { - voxel_size_x_ = static_cast(declare_parameter("voxel_size_x", 0.3)); - voxel_size_y_ = static_cast(declare_parameter("voxel_size_y", 0.3)); - voxel_size_z_ = static_cast(declare_parameter("voxel_size_z", 0.1)); + voxel_size_x_ = declare_parameter("voxel_size_x"); + voxel_size_y_ = declare_parameter("voxel_size_y"); + voxel_size_z_ = declare_parameter("voxel_size_z"); } using std::placeholders::_1; From 2debc785861387723c5fd93a6a984d8c1b9a9ac7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dawid=20Moszy=C5=84ski?= <121798334+dmoszynski@users.noreply.github.com> Date: Wed, 2 Oct 2024 06:58:21 +0200 Subject: [PATCH 21/27] fix(autoware_behavior_velocity_planner_common): add node clock, fix use sim time (#8876) Signed-off-by: Dawid Moszynski --- .../behavior_velocity_planner_common/planner_data.hpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 6e7f8b679d32a..4075f356c187a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -51,7 +51,8 @@ class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) + : clock_(node.get_clock()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { max_stop_acceleration_threshold = node.declare_parameter( "max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml? @@ -60,6 +61,8 @@ struct PlannerData delay_response_time = node.declare_parameter("delay_response_time"); } + rclcpp::Clock::SharedPtr clock_; + // msgs from callbacks that are used for data-ready geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; @@ -108,7 +111,7 @@ struct PlannerData } // Get velocities within stop_duration - const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + const auto now = clock_->now(); std::vector vs; for (const auto & velocity : velocity_buffer) { vs.push_back(velocity.twist.linear.x); From 469f8278603c1202c4500912e8be2a7e66baed94 Mon Sep 17 00:00:00 2001 From: kobayu858 <129580202+kobayu858@users.noreply.github.com> Date: Wed, 2 Oct 2024 14:58:14 +0900 Subject: [PATCH 22/27] fix(autoware_behavior_path_avoidance_by_lane_change_module): fix unmatchedSuppression (#8987) fix:unmatchedSuppression Signed-off-by: kobayu858 --- .../src/interface.cpp | 2 -- .../src/manager.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index d47f76e399b4c..1051a3460be3b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -44,14 +44,12 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( { } -// cppcheck-suppress unusedFunction bool AvoidanceByLaneChangeInterface::isExecutionRequested() const { return module_type_->isLaneChangeRequired() && module_type_->specialRequiredCheck() && module_type_->isValidPath(); } -// cppcheck-suppress unusedFunction void AvoidanceByLaneChangeInterface::processOnEntry() { waitApproval(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index ea5b853051e9a..28ff6c9e110d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -186,7 +186,6 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) avoidance_parameters_ = std::make_shared(p); } -// cppcheck-suppress unusedFunction SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( From 2b179f44d647a597577667aef43c7df595a6f890 Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 2 Oct 2024 15:16:04 +0900 Subject: [PATCH 23/27] feat(autoware_behavior_path_planner_common): disable feature of turning off blinker at low velocity (#9005) * feat(turn_signal_decider): disable feature of turning off blinker at low velocity Signed-off-by: Kyoichi Sugahara --------- Signed-off-by: Kyoichi Sugahara --- .../src/turn_signal_decider.cpp | 24 +++++++++++++++---- 1 file changed, 19 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index 0e005166a97c0..22d80da2cfa67 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -138,11 +138,12 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const size_t current_seg_idx, const RouteHandler & route_handler, const double nearest_dist_threshold, const double nearest_yaw_threshold) { - const auto requires_turn_signal = [&](const auto & lane_attribute) { + const auto requires_turn_signal = [¤t_vel]( + const auto & turn_direction, const bool is_in_turn_lane) { constexpr double stop_velocity_threshold = 0.1; return ( - lane_attribute == "right" || lane_attribute == "left" || - (lane_attribute == "straight" && current_vel < stop_velocity_threshold)); + turn_direction == "right" || turn_direction == "left" || + (turn_direction == "straight" && current_vel < stop_velocity_threshold && !is_in_turn_lane)); }; // base search distance const double base_search_distance = @@ -160,6 +161,19 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } } + bool is_in_turn_lane = false; + for (const auto & lane_id : unique_lane_ids) { + const auto lanelet = route_handler.getLaneletsFromId(lane_id); + const std::string turn_direction = lanelet.attributeOr("turn_direction", "none"); + if (turn_direction == "left" || turn_direction == "right") { + const auto & position = current_pose.position; + const lanelet::BasicPoint2d point(position.x, position.y); + if (lanelet::geometry::inside(lanelet, point)) { + is_in_turn_lane = true; + break; + } + } + } // combine consecutive lanes of the same turn direction // stores lanes that have already been combine std::set processed_lanes; @@ -175,7 +189,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( // Get the lane and its attribute const std::string lane_attribute = current_lane.attributeOr("turn_direction", std::string("none")); - if (!requires_turn_signal(lane_attribute)) continue; + if (!requires_turn_signal(lane_attribute, is_in_turn_lane)) continue; do { processed_lanes.insert(current_lane.id()); @@ -256,7 +270,7 @@ std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( } else if (search_distance <= dist_to_front_point) { continue; } - if (requires_turn_signal(lane_attribute)) { + if (requires_turn_signal(lane_attribute, is_in_turn_lane)) { // update map if necessary if (desired_start_point_map_.find(lane_id) == desired_start_point_map_.end()) { desired_start_point_map_.emplace(lane_id, current_pose); From 5b8219eae6e6370af0589a29efcae3bb9da2542e Mon Sep 17 00:00:00 2001 From: danielsanchezaran Date: Wed, 2 Oct 2024 17:53:50 +0900 Subject: [PATCH 24/27] feat(autonomous_emergency_braking): set max imu path length (#9004) * set a limit to the imu path length Signed-off-by: Daniel Sanchez * fix test and add a new one Signed-off-by: Daniel Sanchez * update readme Signed-off-by: Daniel Sanchez * pre-commit Signed-off-by: Daniel Sanchez * use velocity and time directly to get arc length Signed-off-by: Daniel Sanchez * refactor to reduce repeated code Signed-off-by: Daniel Sanchez * cleaning code Signed-off-by: Daniel Sanchez --------- Signed-off-by: Daniel Sanchez --- .../README.md | 3 +- .../autonomous_emergency_braking.param.yaml | 3 +- .../autonomous_emergency_braking/node.hpp | 3 +- .../src/node.cpp | 51 ++++++++++--------- .../test/test.cpp | 22 +++++++- 5 files changed, 53 insertions(+), 29 deletions(-) diff --git a/control/autoware_autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md index c3583982dde39..42b3f4c9f96de 100644 --- a/control/autoware_autonomous_emergency_braking/README.md +++ b/control/autoware_autonomous_emergency_braking/README.md @@ -213,7 +213,8 @@ When vehicle odometry information is faulty, it is possible that the MPC fails t | cluster_minimum_height | [m] | double | at least one point in a cluster must be higher than this value for the cluster to be included in the set of possible collision targets | 0.1 | | minimum_cluster_size | [-] | int | minimum required amount of points contained by a cluster for it to be considered as a possible target obstacle | 10 | | maximum_cluster_size | [-] | int | maximum amount of points contained by a cluster for it to be considered as a possible target obstacle | 10000 | -| min_generated_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| min_generated_imu_path_length | [m] | double | minimum distance for a predicted path generated by sensors | 0.5 | +| max_generated_imu_path_length | [m] | double | maximum distance for a predicted path generated by sensors | 10.0 | | 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 | diff --git a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml index 75b54fe547e32..f7548536beaef 100644 --- a/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml +++ b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml @@ -7,7 +7,8 @@ use_predicted_object_data: false use_object_velocity_calculation: true check_autoware_state: true - min_generated_path_length: 0.5 + min_generated_imu_path_length: 0.5 + max_generated_imu_path_length: 10.0 imu_prediction_time_horizon: 1.5 imu_prediction_time_interval: 0.1 mpc_prediction_time_horizon: 4.5 diff --git a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp index 6daa0a34dbaf4..214a6dc309210 100644 --- a/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware/autonomous_emergency_braking/node.hpp @@ -562,7 +562,8 @@ class AEB : public rclcpp::Node double voxel_grid_x_; double voxel_grid_y_; double voxel_grid_z_; - double min_generated_path_length_; + double min_generated_imu_path_length_; + double max_generated_imu_path_length_; double expand_width_; double longitudinal_offset_; double t_response_; diff --git a/control/autoware_autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp index 34a7453b51726..b3988ba238fe7 100644 --- a/control/autoware_autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -165,7 +165,8 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) voxel_grid_x_ = declare_parameter("voxel_grid_x"); voxel_grid_y_ = declare_parameter("voxel_grid_y"); voxel_grid_z_ = declare_parameter("voxel_grid_z"); - min_generated_path_length_ = declare_parameter("min_generated_path_length"); + min_generated_imu_path_length_ = declare_parameter("min_generated_imu_path_length"); + max_generated_imu_path_length_ = declare_parameter("max_generated_imu_path_length"); expand_width_ = declare_parameter("expand_width"); longitudinal_offset_ = declare_parameter("longitudinal_offset"); t_response_ = declare_parameter("t_response"); @@ -227,7 +228,8 @@ rcl_interfaces::msg::SetParametersResult AEB::onParameter( updateParam(parameters, "voxel_grid_x", voxel_grid_x_); updateParam(parameters, "voxel_grid_y", voxel_grid_y_); updateParam(parameters, "voxel_grid_z", voxel_grid_z_); - updateParam(parameters, "min_generated_path_length", min_generated_path_length_); + updateParam(parameters, "min_generated_imu_path_length", min_generated_imu_path_length_); + updateParam(parameters, "max_generated_imu_path_length", max_generated_imu_path_length_); updateParam(parameters, "expand_width", expand_width_); updateParam(parameters, "longitudinal_offset", longitudinal_offset_); updateParam(parameters, "t_response", t_response_); @@ -639,39 +641,35 @@ Path AEB::generateEgoPath(const double curr_v, const double curr_w) ini_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); ini_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); path.push_back(ini_pose); - - if (std::abs(curr_v) < 0.1) { - // if current velocity is too small, assume it stops at the same point + const double & dt = imu_prediction_time_interval_; + const double distance_between_points = curr_v * dt; + constexpr double minimum_distance_between_points{1e-2}; + // if current velocity is too small, assume it stops at the same point + // if distance between points is too small, arc length calculation is unreliable, so we skip + // creating the path + if (std::abs(curr_v) < 0.1 || distance_between_points < minimum_distance_between_points) { return path; } - constexpr double epsilon = std::numeric_limits::epsilon(); - 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; - curr_yaw = curr_yaw + curr_w * dt; - geometry_msgs::msg::Pose current_pose; - current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); - current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } - path.push_back(current_pose); - } + double path_arc_length = 0.0; + double t = 0.0; - // If path is shorter than minimum path length - while (autoware::motion_utils::calcArcLength(path) < min_generated_path_length_) { + bool finished_creating_path = false; + while (!finished_creating_path) { curr_x = curr_x + curr_v * std::cos(curr_yaw) * dt; curr_y = curr_y + curr_v * std::sin(curr_yaw) * dt; curr_yaw = curr_yaw + curr_w * dt; geometry_msgs::msg::Pose current_pose; current_pose.position = autoware::universe_utils::createPoint(curr_x, curr_y, 0.0); current_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(curr_yaw); - if (autoware::universe_utils::calcDistance2d(path.back(), current_pose) < 1e-2) { - continue; - } + + t += dt; + path_arc_length += distance_between_points; + + finished_creating_path = (t > horizon) && (path_arc_length > min_generated_imu_path_length_); + finished_creating_path = + (finished_creating_path) || (path_arc_length > max_generated_imu_path_length_); path.push_back(current_pose); } return path; @@ -691,12 +689,15 @@ std::optional AEB::generateEgoPath(const Trajectory & predicted_traj) time_keeper_->start_track("createPath"); Path path; path.reserve(predicted_traj.points.size()); + constexpr double minimum_distance_between_points{1e-2}; for (size_t i = 0; i < predicted_traj.points.size(); ++i) { geometry_msgs::msg::Pose map_pose; tf2::doTransform(predicted_traj.points.at(i).pose, map_pose, transform_stamped.value()); // skip points that are too close to the last point in the path - if (autoware::universe_utils::calcDistance2d(path.back(), map_pose) < 1e-2) { + if ( + autoware::universe_utils::calcDistance2d(path.back(), map_pose) < + minimum_distance_between_points) { continue; } diff --git a/control/autoware_autonomous_emergency_braking/test/test.cpp b/control/autoware_autonomous_emergency_braking/test/test.cpp index 609a7a36b7d67..c2a58941cc144 100644 --- a/control/autoware_autonomous_emergency_braking/test/test.cpp +++ b/control/autoware_autonomous_emergency_braking/test/test.cpp @@ -171,7 +171,7 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) { const double dt = aeb_node_->imu_prediction_time_interval_; const double horizon = aeb_node_->imu_prediction_time_horizon_; - const double min_generated_path_length = aeb_node_->min_generated_path_length_; + const double min_generated_path_length = aeb_node_->min_generated_imu_path_length_; const double slow_velocity = min_generated_path_length / (2.0 * horizon); constexpr double yaw_rate = 0.05; const auto imu_path = aeb_node_->generateEgoPath(slow_velocity, yaw_rate); @@ -185,6 +185,26 @@ TEST_F(TestAEB, checkIncompleteImuPathGeneration) ASSERT_TRUE(footprint.size() == imu_path.size() - 1); } +TEST_F(TestAEB, checkImuPathGenerationIsCut) +{ + const double dt = aeb_node_->imu_prediction_time_interval_; + const double horizon = aeb_node_->imu_prediction_time_horizon_; + const double max_generated_path_length = aeb_node_->max_generated_imu_path_length_; + const double fast_velocity = 2.0 * max_generated_path_length / (horizon); + constexpr double yaw_rate = 0.05; + const auto imu_path = aeb_node_->generateEgoPath(fast_velocity, yaw_rate); + + ASSERT_FALSE(imu_path.empty()); + constexpr double epsilon{1e-3}; + ASSERT_TRUE( + autoware::motion_utils::calcArcLength(imu_path) <= + max_generated_path_length + dt * fast_velocity + epsilon); + + const auto footprint = aeb_node_->generatePathFootprint(imu_path, 0.0); + ASSERT_FALSE(footprint.empty()); + ASSERT_TRUE(footprint.size() == imu_path.size() - 1); +} + TEST_F(TestAEB, checkEmptyPathAtZeroSpeed) { const double velocity = 0.0; From 921be33aa319573f48234e185e256d5f6c46b96c Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 3 Oct 2024 10:39:38 +0900 Subject: [PATCH 25/27] refactor(mission_planner): move anonymous functions to utils and add namespace (#9012) feat(mission_planner): move functions to utils and add namespace Signed-off-by: kosuke55 --- .../src/lanelet2_plugins/default_planner.cpp | 93 +----------------- .../lanelet2_plugins/utility_functions.cpp | 94 +++++++++++++++++++ .../lanelet2_plugins/utility_functions.hpp | 17 ++++ 3 files changed, 112 insertions(+), 92 deletions(-) diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index 3cf6e73bc2bc6..50a6becf2e124 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -41,96 +41,6 @@ #include #include -namespace -{ -using RouteSections = std::vector; - -bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) -{ - // check if goal is on a lane at appropriate angle - const auto distance = boost::geometry::distance( - lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - return distance < th_distance; -} - -bool is_in_parking_space( - const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) -{ - for (const auto & parking_space : parking_spaces) { - lanelet::ConstPolygon3d parking_space_polygon; - if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) { - continue; - } - - const double distance = boost::geometry::distance( - lanelet::utils::to2D(parking_space_polygon).basicPolygon(), - lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - if (distance < th_distance) { - return true; - } - } - return false; -} - -bool is_in_parking_lot( - const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) -{ - for (const auto & parking_lot : parking_lots) { - const double distance = boost::geometry::distance( - lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint()); - constexpr double th_distance = std::numeric_limits::epsilon(); - if (distance < th_distance) { - return true; - } - } - return false; -} - -double project_goal_to_map( - const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) -{ - const lanelet::ConstLineString3d center_line = - lanelet::utils::generateFineCenterline(lanelet_component); - lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint()); - return project.z(); -} - -geometry_msgs::msg::Pose get_closest_centerline_pose( - const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, - autoware::vehicle_info_utils::VehicleInfo vehicle_info) -{ - lanelet::Lanelet closest_lanelet; - if (!lanelet::utils::query::getClosestLaneletWithConstrains( - road_lanelets, point, &closest_lanelet, 0.0)) { - // point is not on any lanelet. - return point; - } - - const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0); - closest_lanelet.setCenterline(refined_center_line); - - const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); - - const auto nearest_idx = autoware::motion_utils::findNearestIndex( - convertCenterlineToPoints(closest_lanelet), point.position); - const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; - - // shift nearest point on its local y axis so that vehicle's right and left edges - // would have approx the same clearance from road border - const auto shift_length = (vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; - const auto delta_x = -shift_length * std::sin(lane_yaw); - const auto delta_y = shift_length * std::cos(lane_yaw); - - lanelet::BasicPoint3d refined_point( - nearest_point.x() + delta_x, nearest_point.y() + delta_y, nearest_point.z()); - - return convertBasicPoint3dToPose(refined_point, lane_yaw); -} - -} // anonymous namespace - namespace autoware::mission_planner::lanelet2 { @@ -395,8 +305,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) std::stringstream log_ss; for (const auto & point : points) { - log_ss << "x: " << point.position.x << " " - << "y: " << point.position.y << std::endl; + log_ss << "x: " << point.position.x << " " << "y: " << point.position.y << std::endl; } RCLCPP_DEBUG_STREAM( logger, "start planning route with check points: " << std::endl diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 34b16ef35f603..08345282a9264 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -14,10 +14,18 @@ #include "utility_functions.hpp" +#include +#include +#include + #include #include +#include + +namespace autoware::mission_planner::lanelet2 +{ autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( autoware::universe_utils::LinearRing2d footprint) { @@ -64,3 +72,89 @@ geometry_msgs::msg::Pose convertBasicPoint3dToPose( return pose; } + +bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point) +{ + // check if goal is on a lane at appropriate angle + const auto distance = boost::geometry::distance( + lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + return distance < th_distance; +} + +bool is_in_parking_space( + const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_space : parking_spaces) { + lanelet::ConstPolygon3d parking_space_polygon; + if (!lanelet::utils::lineStringWithWidthToPolygon(parking_space, &parking_space_polygon)) { + continue; + } + + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_space_polygon).basicPolygon(), + lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +bool is_in_parking_lot( + const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point) +{ + for (const auto & parking_lot : parking_lots) { + const double distance = boost::geometry::distance( + lanelet::utils::to2D(parking_lot).basicPolygon(), lanelet::utils::to2D(point).basicPoint()); + constexpr double th_distance = std::numeric_limits::epsilon(); + if (distance < th_distance) { + return true; + } + } + return false; +} + +double project_goal_to_map( + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point) +{ + const lanelet::ConstLineString3d center_line = + lanelet::utils::generateFineCenterline(lanelet_component); + lanelet::BasicPoint3d project = lanelet::geometry::project(center_line, goal_point.basicPoint()); + return project.z(); +} + +geometry_msgs::msg::Pose get_closest_centerline_pose( + const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, + autoware::vehicle_info_utils::VehicleInfo vehicle_info) +{ + lanelet::Lanelet closest_lanelet; + if (!lanelet::utils::query::getClosestLaneletWithConstrains( + road_lanelets, point, &closest_lanelet, 0.0)) { + // point is not on any lanelet. + return point; + } + + const auto refined_center_line = lanelet::utils::generateFineCenterline(closest_lanelet, 1.0); + closest_lanelet.setCenterline(refined_center_line); + + const double lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, point.position); + + const auto nearest_idx = autoware::motion_utils::findNearestIndex( + convertCenterlineToPoints(closest_lanelet), point.position); + const auto nearest_point = closest_lanelet.centerline()[nearest_idx]; + + // shift nearest point on its local y axis so that vehicle's right and left edges + // would have approx the same clearance from road border + const auto shift_length = (vehicle_info.right_overhang_m - vehicle_info.left_overhang_m) / 2.0; + const auto delta_x = -shift_length * std::sin(lane_yaw); + const auto delta_y = shift_length * std::cos(lane_yaw); + + lanelet::BasicPoint3d refined_point( + nearest_point.x() + delta_x, nearest_point.y() + delta_y, nearest_point.z()); + + return convertBasicPoint3dToPose(refined_point, lane_yaw); +} + +} // namespace autoware::mission_planner::lanelet2 diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index 6e3d2a0e88941..36a2e17fb5ff0 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -29,6 +29,10 @@ #include +namespace autoware::mission_planner::lanelet2 +{ +using RouteSections = std::vector; + template bool exists(const std::vector & vectors, const T & item) { @@ -48,4 +52,17 @@ void insert_marker_array( std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw); + +bool is_in_lane(const lanelet::ConstLanelet & lanelet, const lanelet::ConstPoint3d & point); +bool is_in_parking_space( + const lanelet::ConstLineStrings3d & parking_spaces, const lanelet::ConstPoint3d & point); +bool is_in_parking_lot( + const lanelet::ConstPolygons3d & parking_lots, const lanelet::ConstPoint3d & point); +double project_goal_to_map( + const lanelet::ConstLanelet & lanelet_component, const lanelet::ConstPoint3d & goal_point); +geometry_msgs::msg::Pose get_closest_centerline_pose( + const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, + autoware::vehicle_info_utils::VehicleInfo vehicle_info); + +} // namespace autoware::mission_planner::lanelet2 #endif // LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ From 552786f751933c92f05f18998776b40a102bb1ab Mon Sep 17 00:00:00 2001 From: Taekjin LEE Date: Thu, 3 Oct 2024 10:47:13 +0900 Subject: [PATCH 26/27] refactor(autoware_multi_object_tracker): separate detected object covariance modeling (#9001) * refactor: update object model includes in tracker models Signed-off-by: Taekjin LEE * feat: add uncertainty processor for object tracking feat: refactor uncertainty processing for object tracking feat: impl obj class model feat: Update object model measurement covariances Refactor the object model measurement covariances in the `object_model.hpp` file. Update the velocity long and velocity lat measurement covariances for different object model types. refactor: Model object uncertainty in multi_object_tracker_node.cpp feat: Update object model measurement covariances in object_model.hpp feat: Update uncertainty processing for object tracking fix: remove uncertainty modelling in trackers refactor: Remove unused function isLargeVehicleLabel The function isLargeVehicleLabel in utils.hpp is no longer used and can be safely removed. Revert "refactor: Remove unused function isLargeVehicleLabel" This reverts commit 23e3eff511b21ef8ceeacb7db47c74f747009a32. feat: Normalize uncertainty in object tracking This commit adds a new function `normalizeUncertainty` to the `uncertainty_processor.hpp` and `uncertainty_processor.cpp` files. The function normalizes the position and twist covariance matrices of detected objects to ensure minimum values for distance, radius, and velocity. This helps improve the accuracy and reliability of object tracking. Signed-off-by: Taekjin LEE * refactor: update motion model parameters for object tracking Signed-off-by: Taekjin LEE * refactor: update yaw rate limit in object model Signed-off-by: Taekjin LEE * Revert "refactor: update yaw rate limit in object model" This reverts commit 6e8b201582cb65673678029dc3a781f2b7126f81. Signed-off-by: Taekjin LEE * refactor: update object model measurement covariances Refactor the object model measurement covariances in the `object_model.hpp` file. Update the velocity long and velocity lat measurement covariances for different object model types. Signed-off-by: Taekjin LEE * refactor: update motion model parameters comments Signed-off-by: Taekjin LEE * refactor: remove comment Signed-off-by: Taekjin LEE * style(pre-commit): autofix * feat: Update copyright notice in uncertainty_processor.hpp Update the copyright notice in the uncertainty_processor.hpp file to reflect the correct company name. Signed-off-by: Taekjin LEE * refactor: update runProcess function parameters in multi_object_tracker_node.hpp Signed-off-by: Taekjin LEE --------- Signed-off-by: Taekjin LEE Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 1 + .../object_model/object_model.hpp | 17 ++- .../tracker/model/bicycle_tracker.hpp | 2 +- .../tracker/model/big_vehicle_tracker.hpp | 2 +- .../tracker/model/normal_vehicle_tracker.hpp | 2 +- .../tracker/model/pedestrian_tracker.hpp | 2 +- .../motion_model/bicycle_motion_model.hpp | 35 +++-- .../motion_model/ctrv_motion_model.hpp | 18 +-- .../tracker/motion_model/cv_motion_model.hpp | 16 +- .../uncertainty/uncertainty_processor.hpp | 51 +++++++ .../lib/tracker/model/bicycle_tracker.cpp | 36 ----- .../lib/tracker/model/big_vehicle_tracker.cpp | 44 ------ .../tracker/model/normal_vehicle_tracker.cpp | 44 ------ .../lib/tracker/model/pedestrian_tracker.cpp | 36 ----- .../motion_model/bicycle_motion_model.cpp | 38 +---- .../motion_model/ctrv_motion_model.cpp | 22 +-- .../tracker/motion_model/cv_motion_model.cpp | 18 --- .../lib/uncertainty/uncertainty_processor.cpp | 139 ++++++++++++++++++ .../src/multi_object_tracker_node.cpp | 13 +- .../src/multi_object_tracker_node.hpp | 2 +- 20 files changed, 255 insertions(+), 283 deletions(-) rename perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/{tracker => }/object_model/object_model.hpp (93%) create mode 100644 perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp create mode 100644 perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index fe4546cc9bc60..73edabaa09429 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -44,6 +44,7 @@ set(${PROJECT_NAME}_lib lib/tracker/model/pedestrian_and_bicycle_tracker.cpp lib/tracker/model/unknown_tracker.cpp lib/tracker/model/pass_through_tracker.cpp + lib/uncertainty/uncertainty_processor.cpp ) ament_auto_add_library(${PROJECT_NAME} SHARED ${${PROJECT_NAME}_src} diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp similarity index 93% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp rename to perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp index b49464109eec8..4fea038d00667 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/object_model/object_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/object_model.hpp @@ -16,8 +16,8 @@ // Author: v1.0 Taekjin Lee // -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ #include @@ -53,6 +53,7 @@ namespace object_model { enum class ObjectModelType { NormalVehicle, BigVehicle, Bicycle, Pedestrian, Unknown }; + struct ObjectSize { double length{0.0}; // [m] @@ -287,7 +288,16 @@ class ObjectModel measurement_covariance.pos_x = sq(0.4); measurement_covariance.pos_y = sq(0.4); measurement_covariance.yaw = sq(deg2rad(30.0)); + measurement_covariance.vel_long = sq(kmph2mps(5.0)); + break; + case ObjectModelType::Unknown: + // measurement noise model + measurement_covariance.pos_x = sq(1.0); + measurement_covariance.pos_y = sq(1.0); + measurement_covariance.yaw = sq(deg2rad(360.0)); + measurement_covariance.vel_long = sq(kmph2mps(10.0)); + measurement_covariance.vel_lat = sq(kmph2mps(10.0)); break; default: @@ -302,8 +312,9 @@ static const ObjectModel normal_vehicle(ObjectModelType::NormalVehicle); static const ObjectModel big_vehicle(ObjectModelType::BigVehicle); static const ObjectModel bicycle(ObjectModelType::Bicycle); static const ObjectModel pedestrian(ObjectModelType::Pedestrian); +static const ObjectModel unknown(ObjectModelType::Unknown); } // namespace object_model } // namespace autoware::multi_object_tracker -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__OBJECT_MODEL_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 7cb2963d38ef1..8501c68b0cf23 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp index 227e6cd01f4dc..489f656f750cb 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BIG_VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp index 8f5bab65c6aed..0bfc065c7cc68 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__NORMAL_VEHICLE_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index 500148ba41081..f20b38f73e95f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -20,9 +20,9 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/object_model.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" -#include "autoware/multi_object_tracker/tracker/object_model/object_model.hpp" namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp index 123eb30e63d6c..b84f8a4bd3bd7 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp @@ -45,24 +45,25 @@ class BicycleMotionModel : public MotionModel double lf_; double lr_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_stddev_acc_long; - double q_stddev_acc_lat; - double q_cov_acc_long; - double q_cov_acc_lat; - double q_stddev_yaw_rate_min; - double q_stddev_yaw_rate_max; - double q_cov_slip_rate_min; - double q_cov_slip_rate_max; - double q_max_slip_angle; - double lf_ratio; - double lr_ratio; - double lf_min; - double lr_min; - double max_vel; - double max_slip; + double q_stddev_acc_long = 3.43; // [m/s^2] uncertain longitudinal acceleration, 0.35G + double q_stddev_acc_lat = 1.47; // [m/s^2] uncertain longitudinal acceleration, 0.15G + double q_cov_acc_long = 11.8; // [m/s^2] uncertain longitudinal acceleration, 0.35G + double q_cov_acc_lat = 2.16; // [m/s^2] uncertain lateral acceleration, 0.15G + double q_stddev_yaw_rate_min = 0.02618; // [rad/s] uncertain yaw change rate, 1.5deg/s + double q_stddev_yaw_rate_max = 0.2618; // [rad/s] uncertain yaw change rate, 15deg/s + double q_cov_slip_rate_min = + 2.7416e-5; // [rad^2/s^2] uncertain slip angle change rate, 0.3 deg/s + double q_cov_slip_rate_max = 0.03046; // [rad^2/s^2] uncertain slip angle change rate, 10 deg/s + double q_max_slip_angle = 0.5236; // [rad] max slip angle, 30deg + double lf_ratio = 0.3; // [-] ratio of the distance from the center to the front wheel + double lr_ratio = 0.25; // [-] ratio of the distance from the center to the rear wheel + double lf_min = 1.0; // [m] minimum distance from the center to the front wheel + double lr_min = 1.0; // [m] minimum distance from the center to the rear wheel + double max_vel = 27.8; // [m/s] maximum velocity, 100km/h + double max_slip = 0.5236; // [rad] maximum slip angle, 30deg } motion_params_; public: @@ -76,8 +77,6 @@ class BicycleMotionModel : public MotionModel const std::array & pose_cov, const double & vel, const double & vel_cov, const double & slip, const double & slip_cov, const double & length); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_acc_long, const double & q_stddev_acc_lat, const double & q_stddev_yaw_rate_min, const double & q_stddev_yaw_rate_max, diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp index 2632d99047053..812b91fc8acf0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp @@ -42,16 +42,16 @@ class CTRVMotionModel : public MotionModel // attributes rclcpp::Logger logger_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x; - double q_cov_y; - double q_cov_yaw; - double q_cov_vel; - double q_cov_wz; - double max_vel; - double max_wz; + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_yaw = 0.1218; // [rad^2/s^2] uncertain yaw angle, 20deg/s + double q_cov_vel = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_wz = 0.5236; // [rad^2/s^4] uncertain yaw rate, 30deg/s^2 + double max_vel = 2.78; // [m/s] maximum velocity + double max_wz = 0.5236; // [rad/s] maximum yaw rate, 30deg/s } motion_params_; public: @@ -65,8 +65,6 @@ class CTRVMotionModel : public MotionModel const std::array & pose_cov, const double & vel, const double & vel_cov, const double & wz, const double & wz_cov); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_yaw, const double & q_stddev_vx, const double & q_stddev_wz); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp index 26799f1916741..dad1b1024879a 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp @@ -41,15 +41,15 @@ class CVMotionModel : public MotionModel // attributes rclcpp::Logger logger_; - // motion parameters + // motion parameters: process noise and motion limits struct MotionParams { - double q_cov_x; - double q_cov_y; - double q_cov_vx; - double q_cov_vy; - double max_vx; - double max_vy; + double q_cov_x = 0.025; // [m^2/s^2] uncertain position in x, 0.5m/s + double q_cov_y = 0.025; // [m^2/s^2] uncertain position in y, 0.5m/s + double q_cov_vx = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double q_cov_vy = 8.6436; // [m^2/s^4] uncertain velocity, 0.3G m/s^2 + double max_vx = 16.67; // [m/s] maximum velocity, 60km/h + double max_vy = 16.67; // [m/s] maximum velocity, 60km/h } motion_params_; public: @@ -63,8 +63,6 @@ class CVMotionModel : public MotionModel const std::array & pose_cov, const double & vx, const double & vy, const std::array & twist_cov); - void setDefaultParams(); - void setMotionParams( const double & q_stddev_x, const double & q_stddev_y, const double & q_stddev_vx, const double & q_stddev_vy); diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp new file mode 100644 index 0000000000000..52781be1b8201 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -0,0 +1,51 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ + +#include "autoware/multi_object_tracker/object_model/object_model.hpp" + +#include + +#include + +namespace autoware::multi_object_tracker +{ + +namespace uncertainty +{ + +using autoware::multi_object_tracker::object_model::ObjectModel; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::ObjectClassification; + +ObjectModel decodeObjectModel(const ObjectClassification & object_class); + +DetectedObjects modelUncertainty(const DetectedObjects & detected_objects); + +object_model::StateCovariance covarianceFromObjectClass( + const DetectedObject & detected_object, const ObjectClassification & object_class); + +void normalizeUncertainty(DetectedObjects & detected_objects); + +} // namespace uncertainty + +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index ba53ccec1ad43..38a49e48d8d10 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -162,42 +162,6 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( } } - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp index 0593b7fc9dc12..d5913eccdf0bf 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/big_vehicle_tracker.cpp @@ -193,50 +193,6 @@ autoware_perception_msgs::msg::DetectedObject BigVehicleTracker::getUpdatingObje bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (label == autoware_perception_msgs::msg::ObjectClassification::CAR) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = r_stddev_x * r_stddev_x; - r_cov_y = r_stddev_y * r_stddev_y; - } - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp index ef345692b32ca..a9260201722b3 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/normal_vehicle_tracker.cpp @@ -195,50 +195,6 @@ autoware_perception_msgs::msg::DetectedObject NormalVehicleTracker::getUpdatingO bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - const uint8_t label = object_recognition_utils::getHighestProbLabel(object.classification); - if (utils::isLargeVehicleLabel(label)) { - // if label is changed, enlarge the measurement noise covariance - constexpr float r_stddev_x = 2.0; // [m] - constexpr float r_stddev_y = 2.0; // [m] - r_cov_x = r_stddev_x * r_stddev_x; - r_cov_y = r_stddev_y * r_stddev_y; - } - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index ee50c2e5449ed..591453a14f116 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -154,42 +154,6 @@ autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObje { autoware_perception_msgs::msg::DetectedObject updating_object = object; - // UNCERTAINTY MODEL - if (!object.kinematics.has_position_covariance) { - // measurement noise covariance - auto r_cov_x = object_model_.measurement_covariance.pos_x; - auto r_cov_y = object_model_.measurement_covariance.pos_y; - - // yaw angle fix - const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; - - // fill covariance matrix - using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; - const double cos_yaw = std::cos(pose_yaw); - const double sin_yaw = std::sin(pose_yaw); - const double sin_2yaw = std::sin(2.0 * pose_yaw); - pose_cov[XYZRPY_COV_IDX::X_X] = - r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x - pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y - pose_cov[XYZRPY_COV_IDX::Y_Y] = - r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y - pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x - pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw - pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw - pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x - pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y - pose_cov[XYZRPY_COV_IDX::YAW_YAW] = object_model_.measurement_covariance.yaw; // yaw - yaw - if (!is_yaw_available) { - pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value - } - auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; - twist_cov[XYZRPY_COV_IDX::X_X] = object_model_.measurement_covariance.vel_long; // vel - vel - } - return updating_object; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 215e55cb4ac62..3c088b8f64b39 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -38,40 +38,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; BicycleMotionModel::BicycleMotionModel() : logger_(rclcpp::get_logger("BicycleMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void BicycleMotionModel::setDefaultParams() -{ - // set default motion parameters - constexpr double q_stddev_acc_long = 9.8 * 0.35; // [m/(s*s)] uncertain longitudinal acceleration - constexpr double q_stddev_acc_lat = 9.8 * 0.15; // [m/(s*s)] uncertain lateral acceleration - constexpr double q_stddev_yaw_rate_min = - autoware::universe_utils::deg2rad(1.5); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_yaw_rate_max = - autoware::universe_utils::deg2rad(15.0); // [rad/s] uncertain yaw change rate - constexpr double q_stddev_slip_rate_min = - autoware::universe_utils::deg2rad(0.3); // [rad/s] uncertain slip angle change rate - constexpr double q_stddev_slip_rate_max = - autoware::universe_utils::deg2rad(10.0); // [rad/s] uncertain slip angle change rate - constexpr double q_max_slip_angle = - autoware::universe_utils::deg2rad(30.0); // [rad] max slip angle - // extended state parameters - constexpr double lf_ratio = 0.3; // 30% front from the center - constexpr double lf_min = 1.0; // minimum of 1.0m - constexpr double lr_ratio = 0.25; // 25% rear from the center - constexpr double lr_min = 1.0; // minimum of 1.0m - setMotionParams( - q_stddev_acc_long, q_stddev_acc_lat, q_stddev_yaw_rate_min, q_stddev_yaw_rate_max, - q_stddev_slip_rate_min, q_stddev_slip_rate_max, q_max_slip_angle, lf_ratio, lf_min, lr_ratio, - lr_min); - - // set motion limitations - constexpr double max_vel = autoware::universe_utils::kmph2mps(100); // [m/s] maximum velocity - constexpr double max_slip = 30.0; // [deg] maximum slip angle - setMotionLimits(max_vel, max_slip); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); @@ -101,8 +67,8 @@ void BicycleMotionModel::setMotionParams( logger_, "BicycleMotionModel::setMotionParams: minimum wheel position should be greater than 0.01m."); } - motion_params_.lf_min = (lf_min < minimum_wheel_pos) ? minimum_wheel_pos : lf_min; - motion_params_.lr_min = (lr_min < minimum_wheel_pos) ? minimum_wheel_pos : lr_min; + motion_params_.lf_min = std::max(minimum_wheel_pos, lf_min); + motion_params_.lr_min = std::max(minimum_wheel_pos, lr_min); motion_params_.lf_ratio = lf_ratio; motion_params_.lr_ratio = lr_ratio; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index bf5950bdd4023..a838bf62e5bcb 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -36,26 +36,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CTRVMotionModel::CTRVMotionModel() : logger_(rclcpp::get_logger("CTRVMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void CTRVMotionModel::setDefaultParams() -{ - // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] - constexpr double q_stddev_y = 0.5; // [m/s] - constexpr double q_stddev_yaw = autoware::universe_utils::deg2rad(20); // [rad/s] - constexpr double q_stddev_vel = 9.8 * 0.3; // [m/(s*s)] - constexpr double q_stddev_wz = autoware::universe_utils::deg2rad(30); // [rad/(s*s)] - - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_yaw, q_stddev_vel, q_stddev_wz); - - // set motion limitations - constexpr double max_vel = autoware::universe_utils::kmph2mps(10); // [m/s] maximum velocity - constexpr double max_wz = 30.0; // [deg] maximum yaw rate - setMotionLimits(max_vel, max_wz); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); @@ -77,7 +57,7 @@ void CTRVMotionModel::setMotionLimits(const double & max_vel, const double & max { // set motion limitations motion_params_.max_vel = max_vel; - motion_params_.max_wz = autoware::universe_utils::deg2rad(max_wz); + motion_params_.max_wz = max_wz; } bool CTRVMotionModel::initialize( diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index 2e2ba660a6e0d..e139419197d79 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -38,24 +38,6 @@ using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; CVMotionModel::CVMotionModel() : logger_(rclcpp::get_logger("CVMotionModel")) { - // Initialize motion parameters - setDefaultParams(); -} - -void CVMotionModel::setDefaultParams() -{ - // process noise covariance - constexpr double q_stddev_x = 0.5; // [m/s] standard deviation of x - constexpr double q_stddev_y = 0.5; // [m/s] standard deviation of y - constexpr double q_stddev_vx = 9.8 * 0.3; // [m/(s*s)] standard deviation of vx - constexpr double q_stddev_vy = 9.8 * 0.3; // [m/(s*s)] standard deviation of vy - setMotionParams(q_stddev_x, q_stddev_y, q_stddev_vx, q_stddev_vy); - - // set motion limitations - constexpr double max_vx = autoware::universe_utils::kmph2mps(60); // [m/s] maximum x velocity - constexpr double max_vy = autoware::universe_utils::kmph2mps(60); // [m/s] maximum y velocity - setMotionLimits(max_vx, max_vy); - // set prediction parameters constexpr double dt_max = 0.11; // [s] maximum time interval for prediction setMaxDeltaTime(dt_max); diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp new file mode 100644 index 0000000000000..6770b298bdbda --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -0,0 +1,139 @@ +// Copyright 2024 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" + +namespace autoware::multi_object_tracker +{ +namespace uncertainty +{ +using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + +object_model::StateCovariance covarianceFromObjectClass(const ObjectClassification & object_class) +{ + const auto & label = object_class.label; + ObjectModel obj_class_model(object_model::ObjectModelType::Unknown); + switch (label) { + case ObjectClassification::CAR: + obj_class_model = object_model::normal_vehicle; + break; + case ObjectClassification::BUS: + case ObjectClassification::TRUCK: + case ObjectClassification::TRAILER: + obj_class_model = object_model::big_vehicle; + break; + case ObjectClassification::BICYCLE: + case ObjectClassification::MOTORCYCLE: + obj_class_model = object_model::bicycle; + break; + case ObjectClassification::PEDESTRIAN: + obj_class_model = object_model::pedestrian; + break; + default: + obj_class_model = object_model::normal_vehicle; + break; + } + return obj_class_model.measurement_covariance; +} + +DetectedObject modelUncertaintyByClass( + const DetectedObject & object, const ObjectClassification & object_class) +{ + DetectedObject updating_object = object; + + // measurement noise covariance + const object_model::StateCovariance measurement_covariance = + covarianceFromObjectClass(object_class); + + const auto & r_cov_x = measurement_covariance.pos_x; + const auto & r_cov_y = measurement_covariance.pos_y; + + // yaw angle + const double pose_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // fill position covariance matrix + auto & pose_cov = updating_object.kinematics.pose_with_covariance.covariance; + const double cos_yaw = std::cos(pose_yaw); + const double sin_yaw = std::sin(pose_yaw); + const double sin_2yaw = std::sin(2.0 * pose_yaw); + pose_cov[XYZRPY_COV_IDX::X_X] = + r_cov_x * cos_yaw * cos_yaw + r_cov_y * sin_yaw * sin_yaw; // x - x + pose_cov[XYZRPY_COV_IDX::X_Y] = 0.5 * (r_cov_x - r_cov_y) * sin_2yaw; // x - y + pose_cov[XYZRPY_COV_IDX::Y_Y] = + r_cov_x * sin_yaw * sin_yaw + r_cov_y * cos_yaw * cos_yaw; // y - y + pose_cov[XYZRPY_COV_IDX::Y_X] = pose_cov[XYZRPY_COV_IDX::X_Y]; // y - x + pose_cov[XYZRPY_COV_IDX::X_YAW] = 0.0; // x - yaw + pose_cov[XYZRPY_COV_IDX::Y_YAW] = 0.0; // y - yaw + pose_cov[XYZRPY_COV_IDX::YAW_X] = 0.0; // yaw - x + pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = measurement_covariance.yaw; // yaw - yaw + const bool is_yaw_available = + object.kinematics.orientation_availability != + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + if (!is_yaw_available) { + pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value + } + + // fill twist covariance matrix + auto & twist_cov = updating_object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = measurement_covariance.vel_long; + twist_cov[XYZRPY_COV_IDX::X_Y] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_X] = 0.0; + twist_cov[XYZRPY_COV_IDX::Y_Y] = measurement_covariance.vel_lat; + + return updating_object; +} + +DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) +{ + DetectedObjects updating_objects; + updating_objects.header = detected_objects.header; + for (const auto & object : detected_objects.objects) { + if (object.kinematics.has_position_covariance) { + updating_objects.objects.push_back(object); + continue; + } + const ObjectClassification & object_class = + object_recognition_utils::getHighestProbClassification(object.classification); + updating_objects.objects.push_back(modelUncertaintyByClass(object, object_class)); + } + return updating_objects; +} + +void normalizeUncertainty(DetectedObjects & detected_objects) +{ + constexpr double min_cov_dist = 1e-4; + constexpr double min_cov_rad = 1e-6; + constexpr double min_cov_vel = 1e-4; + + for (auto & object : detected_objects.objects) { + // normalize position covariance + auto & pose_cov = object.kinematics.pose_with_covariance.covariance; + pose_cov[XYZRPY_COV_IDX::X_X] = std::max(pose_cov[XYZRPY_COV_IDX::X_X], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::Y_Y] = std::max(pose_cov[XYZRPY_COV_IDX::Y_Y], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::Z_Z] = std::max(pose_cov[XYZRPY_COV_IDX::Z_Z], min_cov_dist); + pose_cov[XYZRPY_COV_IDX::YAW_YAW] = std::max(pose_cov[XYZRPY_COV_IDX::YAW_YAW], min_cov_rad); + + // normalize twist covariance + auto & twist_cov = object.kinematics.twist_with_covariance.covariance; + twist_cov[XYZRPY_COV_IDX::X_X] = std::max(twist_cov[XYZRPY_COV_IDX::X_X], min_cov_vel); + twist_cov[XYZRPY_COV_IDX::Y_Y] = std::max(twist_cov[XYZRPY_COV_IDX::Y_Y], min_cov_vel); + } +} + +} // namespace uncertainty +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index ce7f8157a31b6..e5ffddb31ccce 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -17,6 +17,7 @@ #include "multi_object_tracker_node.hpp" +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" #include "autoware/multi_object_tracker/utils/utils.hpp" #include @@ -260,11 +261,11 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list) } void MultiObjectTracker::runProcess( - const DetectedObjects & input_objects_msg, const uint & channel_index) + const DetectedObjects & input_objects, const uint & channel_index) { // Get the time of the measurement const rclcpp::Time measurement_time = - rclcpp::Time(input_objects_msg.header.stamp, this->now().get_clock_type()); + rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); // Get the transform of the self frame const auto self_transform = @@ -273,10 +274,16 @@ void MultiObjectTracker::runProcess( return; } + // Model the object uncertainty if it is empty + DetectedObjects input_objects_with_uncertainty = uncertainty::modelUncertainty(input_objects); + + // Normalize the object uncertainty + uncertainty::normalizeUncertainty(input_objects_with_uncertainty); + // Transform the objects to the world frame DetectedObjects transformed_objects; if (!object_recognition_utils::transformObjects( - input_objects_msg, world_frame_id_, tf_buffer_, transformed_objects)) { + input_objects_with_uncertainty, world_frame_id_, tf_buffer_, transformed_objects)) { return; } diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 3c23e3f066488..04d83ebd47acb 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -100,7 +100,7 @@ class MultiObjectTracker : public rclcpp::Node void onMessage(const ObjectsList & objects_list); // publish processes - void runProcess(const DetectedObjects & input_objects_msg, const uint & channel_index); + void runProcess(const DetectedObjects & input_objects, const uint & channel_index); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; From 8f09a112b778b5deadda8c3b2e6b1ca15bdf0f0c Mon Sep 17 00:00:00 2001 From: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Thu, 3 Oct 2024 04:58:24 +0300 Subject: [PATCH 27/27] fix(tier4_control_launch): fix aeb input predicted object topic name (#8874) fix aeb input predicted object topic Signed-off-by: ismetatabay --- launch/tier4_control_launch/launch/control.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index 0eda6faaf4e05..cc24f339ed404 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -192,7 +192,7 @@ - +