Skip to content

Commit

Permalink
feat(static_obstacle_avoidance): update envelope polygon creation met…
Browse files Browse the repository at this point in the history
…hod (autowarefoundation#8551)

* update envelope polygon by considering pose covariance

Signed-off-by: Go Sakayori <[email protected]>

* change parameter

Signed-off-by: Go Sakayori <[email protected]>

* modify schema json

Signed-off-by: Go Sakayori <[email protected]>

* Update planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp

Co-authored-by: Satoshi OTA <[email protected]>

---------

Signed-off-by: Go Sakayori <[email protected]>
Signed-off-by: Go Sakayori <[email protected]>
Co-authored-by: Satoshi OTA <[email protected]>
  • Loading branch information
go-sakayori and satoshi-ota committed Aug 28, 2024
1 parent 83aa6bf commit 4de049b
Show file tree
Hide file tree
Showing 8 changed files with 107 additions and 8 deletions.
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
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
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
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
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_
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
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
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 @@ -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 4de049b

Please sign in to comment.