From 5d7305bc5e398ffb222ec946d8c5b251b0184967 Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 21 Aug 2024 14:36:49 +0900 Subject: [PATCH 1/2] fix(static_obstacle_avoidance): target object sorting (#8545) * fix compensateLostTargetObjects function Signed-off-by: Go Sakayori * remove empty case Signed-off-by: Go Sakayori --------- Signed-off-by: Go Sakayori --- .../src/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index f7bda56150b49..79ba00cdf4d94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -1674,8 +1674,8 @@ void compensateLostTargetObjects( const std::shared_ptr & parameters) { const auto include = [](const auto & objects, const auto & search_id) { - return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) { - return o.object.object_id != search_id; + return std::any_of(objects.begin(), objects.end(), [&search_id](const auto & o) { + return o.object.object_id == search_id; }); }; From 071034352c372d250f9ae0b4a136c460d757473d Mon Sep 17 00:00:00 2001 From: Go Sakayori Date: Wed, 21 Aug 2024 18:01:15 +0900 Subject: [PATCH 2/2] feat(static_obstacle_avoidance): update envelope polygon creation method (#8551) * update envelope polygon by considering pose covariance Signed-off-by: Go Sakayori * change parameter Signed-off-by: Go Sakayori * modify schema json Signed-off-by: Go Sakayori * Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --------- Signed-off-by: Go Sakayori Signed-off-by: Go Sakayori Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../static_obstacle_avoidance.param.yaml | 8 +++ .../data_structs.hpp | 5 ++ .../parameter_helper.hpp | 2 + .../type_alias.hpp | 1 + .../utils.hpp | 2 + .../static_obstacle_avoidance.schema.json | 64 ++++++++++++++++--- .../src/manager.cpp | 2 + .../src/utils.cpp | 31 +++++++++ 8 files changed, 107 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml index 0e60e3216361d..8c9dff8ece448 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml @@ -28,6 +28,7 @@ hard_margin_for_parked_vehicle: 0.7 # [m] max_expand_ratio: 0.0 # [-] FOR DEVELOPER envelope_buffer_margin: 0.5 # [m] FOR DEVELOPER + th_error_eclipse_long_radius : 0.6 # [m] truck: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -38,6 +39,7 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 bus: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -48,6 +50,7 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 trailer: th_moving_speed: 1.0 th_moving_time: 2.0 @@ -58,6 +61,7 @@ hard_margin_for_parked_vehicle: 0.7 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 unknown: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -68,6 +72,7 @@ hard_margin_for_parked_vehicle: -0.2 max_expand_ratio: 0.0 envelope_buffer_margin: 0.1 + th_error_eclipse_long_radius : 0.6 bicycle: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -78,6 +83,7 @@ hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 motorcycle: th_moving_speed: 1.0 th_moving_time: 1.0 @@ -88,6 +94,7 @@ hard_margin_for_parked_vehicle: 0.3 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 pedestrian: th_moving_speed: 0.28 th_moving_time: 1.0 @@ -98,6 +105,7 @@ hard_margin_for_parked_vehicle: 0.5 max_expand_ratio: 0.0 envelope_buffer_margin: 0.5 + th_error_eclipse_long_radius : 0.6 lower_distance_for_polygon_expansion: 30.0 # [m] FOR DEVELOPER upper_distance_for_polygon_expansion: 100.0 # [m] FOR DEVELOPER diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp index 4e14dc4886768..b530da909c59d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -90,6 +90,8 @@ struct ObjectParameter double lateral_hard_margin_for_parked_vehicle{1.0}; double longitudinal_margin{0.0}; + + double th_error_eclipse_long_radius{0.0}; }; struct AvoidanceParameters @@ -420,6 +422,9 @@ struct ObjectData // avoidance target // to stop line distance double to_stop_line{std::numeric_limits::infinity()}; + // long radius of the covariance error ellipse + double error_eclipse_max{std::numeric_limits::infinity()}; + // if lateral margin is NOT enough, the ego must avoid the object. bool avoid_required{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 56ac3d7f4f1bb..b56c48d15df24 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -69,6 +69,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node) param.lateral_hard_margin_for_parked_vehicle = getOrDeclareParameter(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle"); param.longitudinal_margin = getOrDeclareParameter(*node, ns + "longitudinal_margin"); + param.th_error_eclipse_long_radius = + getOrDeclareParameter(*node, ns + "th_error_eclipse_long_radius"); return param; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp index cfbd3f89308ac..d9055bc8a1c34 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -41,6 +41,7 @@ using tier4_planning_msgs::msg::PathWithLaneId; // ROS 2 general msgs using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseWithCovariance; using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp index 909d28ed4bab6..66e8ee550fb2a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -174,6 +174,8 @@ double calcDistanceToAvoidStartLine( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); +double calcErrorEclipseLongRadius(const PoseWithCovariance & pose); + } // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance #endif // AUTOWARE__BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index 38a91ac39525b..eb80c705cfdc2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -96,6 +96,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -104,7 +109,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, @@ -158,6 +164,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -166,7 +177,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, @@ -220,6 +232,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -228,7 +245,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, @@ -282,6 +300,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -290,7 +313,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, @@ -344,6 +368,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -352,7 +381,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, @@ -406,6 +436,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -414,7 +449,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, @@ -468,6 +504,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -476,7 +517,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, @@ -530,6 +572,11 @@ "type": "number", "description": "This value will be applied envelope_buffer_margin according to the distance between the ego and object.", "default": 0.0 + }, + "th_error_eclipse_long_radius": { + "type": "number", + "description": "This value will be applied to judge whether the eclipse error is to large", + "default": 0.6 } }, "required": [ @@ -538,7 +585,8 @@ "longitudinal_margin", "lateral_margin", "envelope_buffer_margin", - "max_expand_ratio" + "max_expand_ratio", + "th_error_eclipse_long_radius" ], "additionalProperties": false }, diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index 5655696ff086d..ddc425fd1520b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -60,6 +60,8 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams( parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle", config.lateral_hard_margin_for_parked_vehicle); updateParam(parameters, ns + "longitudinal_margin", config.longitudinal_margin); + updateParam( + parameters, ns + "th_error_eclipse_long_radius", config.th_error_eclipse_long_radius); }; { diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 79ba00cdf4d94..3729188ff9294 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -21,6 +21,7 @@ #include "autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp" +#include #include #include @@ -1500,11 +1501,27 @@ void fillObjectEnvelopePolygon( if (same_id_obj == registered_objects.end()) { object_data.envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + object_data.error_eclipse_max = + calcErrorEclipseLongRadius(object_data.object.kinematics.initial_pose_with_covariance); return; } const auto one_shot_envelope_poly = createEnvelopePolygon(object_data, closest_pose, envelope_buffer_margin); + const double error_eclipse_long_radius = + calcErrorEclipseLongRadius(object_data.object.kinematics.initial_pose_with_covariance); + + if (error_eclipse_long_radius > object_parameter.th_error_eclipse_long_radius) { + if (error_eclipse_long_radius < object_data.error_eclipse_max) { + object_data.error_eclipse_max = error_eclipse_long_radius; + object_data.envelope_poly = one_shot_envelope_poly; + return; + } + object_data.envelope_poly = same_id_obj->envelope_poly; + return; + } + + object_data.error_eclipse_max = error_eclipse_long_radius; // If the one_shot_envelope_poly is within the registered envelope, use the registered one if (boost::geometry::within(one_shot_envelope_poly, same_id_obj->envelope_poly)) { @@ -2512,4 +2529,18 @@ double calcDistanceToReturnDeadLine( return distance_to_return_dead_line; } + +double calcErrorEclipseLongRadius(const PoseWithCovariance & pose) +{ + Eigen::Matrix2d xy_covariance; + const auto cov = pose.covariance; + xy_covariance(0, 0) = cov[0 * 6 + 0]; + xy_covariance(0, 1) = cov[0 * 6 + 1]; + xy_covariance(1, 0) = cov[1 * 6 + 0]; + xy_covariance(1, 1) = cov[1 * 6 + 1]; + + Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); + + return std::sqrt(eigensolver.eigenvalues()(1)); +} } // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance