Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(static_obstacle_avoidance): cherry-pick #8545, #8551 #1499

Merged
merged 2 commits into from
Sep 3, 2024
Merged

fix(static_obstacle_avoidance): cherry-pick #8545, #8551 #1499

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5d7305b
fix(static_obstacle_avoidance): target object sorting (#8545)
go-sakayori Aug 21, 2024
0710343
feat(static_obstacle_avoidance): update envelope polygon creation met…
go-sakayori Aug 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions ...ehavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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

Expand Down
5 changes: 5 additions & 0 deletions ...e_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -420,6 +422,9 @@ struct ObjectData // avoidance target
// to stop line distance
double to_stop_line{std::numeric_limits<double>::infinity()};

// long radius of the covariance error ellipse
double error_eclipse_max{std::numeric_limits<double>::infinity()};

// if lateral margin is NOT enough, the ego must avoid the object.
bool avoid_required{false};

Expand Down
2 changes: 2 additions & 0 deletions ...dule/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
param.lateral_hard_margin_for_parked_vehicle =
getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
param.longitudinal_margin = getOrDeclareParameter<double>(*node, ns + "longitudinal_margin");
param.th_error_eclipse_long_radius =
getOrDeclareParameter<double>(*node, ns + "th_error_eclipse_long_radius");
return param;
};

Expand Down
1 change: 1 addition & 0 deletions ...nce_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
2 changes: 2 additions & 0 deletions ...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,8 @@ double calcDistanceToAvoidStartLine(
const std::shared_ptr<const PlannerData> & planner_data,
const std::shared_ptr<AvoidanceParameters> & 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_
64 changes: 56 additions & 8 deletions ...havior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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": [
Expand All @@ -104,7 +109,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down Expand Up @@ -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": [
Expand All @@ -166,7 +177,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down Expand Up @@ -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": [
Expand All @@ -228,7 +245,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down Expand Up @@ -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": [
Expand All @@ -290,7 +313,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down Expand Up @@ -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": [
Expand All @@ -352,7 +381,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down Expand Up @@ -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": [
Expand All @@ -414,7 +449,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down Expand Up @@ -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": [
Expand All @@ -476,7 +517,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down Expand Up @@ -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": [
Expand All @@ -538,7 +585,8 @@
"longitudinal_margin",
"lateral_margin",
"envelope_buffer_margin",
"max_expand_ratio"
"max_expand_ratio",
"th_error_eclipse_long_radius"
],
"additionalProperties": false
},
Expand Down
2 changes: 2 additions & 0 deletions ...vior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle",
config.lateral_hard_margin_for_parked_vehicle);
updateParam<double>(parameters, ns + "longitudinal_margin", config.longitudinal_margin);
updateParam<double>(
parameters, ns + "th_error_eclipse_long_radius", config.th_error_eclipse_long_radius);
};

{
Expand Down
35 changes: 33 additions & 2 deletions ...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <Eigen/Dense>
#include <lanelet2_extension/utility/message_conversion.hpp>

#include <boost/geometry/algorithms/buffer.hpp>
Expand Down Expand Up @@ -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)) {
Expand Down Expand Up @@ -1674,8 +1691,8 @@ void compensateLostTargetObjects(
const std::shared_ptr<AvoidanceParameters> & 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;
});
};

Expand Down Expand Up @@ -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<Eigen::Matrix2d> eigensolver(xy_covariance);

return std::sqrt(eigensolver.eigenvalues()(1));
}
} // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance
Loading