Skip to content

Commit

Permalink
Merge pull request #1499 from tier4/fix/beta/v0.29.0-1/static_obstacl…
Browse files Browse the repository at this point in the history 
…e_avoidance

fix(static_obstacle_avoidance): cherry-pick autowarefoundation#8545, autowarefoundation#8551
  • Loading branch information
@kotaro-hihara
kotaro-hihara committed Sep 3, 2024
2 parents d3e1f88 + 0710343 commit fd5dda0
Show file tree
Hide file tree
Showing 8 changed files with 109 additions and 10 deletions.
8 changes: 8 additions & 0 deletions ...ehavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
View file
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
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
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
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
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
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
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
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

0 comments on commit fd5dda0

Please sign in to comment.