|
20 | 20 | #include "behavior_path_planner_common/utils/path_utils.hpp"
|
21 | 21 | #include "behavior_path_planner_common/utils/utils.hpp"
|
22 | 22 |
|
| 23 | +#include <behavior_path_avoidance_module/data_structs.hpp> |
| 24 | +#include <behavior_path_lane_change_module/utils/utils.hpp> |
23 | 25 | #include <lanelet2_extension/utility/utilities.hpp>
|
24 | 26 | #include <rclcpp/logging.hpp>
|
25 | 27 |
|
26 | 28 | #include <boost/geometry/algorithms/centroid.hpp>
|
27 | 29 | #include <boost/geometry/strategies/cartesian/centroid_bashein_detmer.hpp>
|
28 | 30 |
|
| 31 | +#include <limits> |
29 | 32 | #include <utility>
|
30 | 33 |
|
31 | 34 | namespace behavior_path_planner
|
32 | 35 | {
|
| 36 | +using behavior_path_planner::utils::lane_change::debug::createExecutionArea; |
| 37 | + |
33 | 38 | namespace
|
34 | 39 | {
|
35 | 40 | lanelet::BasicLineString3d toLineString3d(const std::vector<Point> & bound)
|
@@ -78,44 +83,15 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
|
78 | 83 | return false;
|
79 | 84 | }
|
80 | 85 |
|
81 |
| - const auto current_lanes = getCurrentLanes(); |
82 |
| - if (current_lanes.empty()) { |
83 |
| - RCLCPP_DEBUG(logger_, "no empty lanes"); |
84 |
| - return false; |
85 |
| - } |
86 |
| - |
87 | 86 | const auto & nearest_object = data.target_objects.front();
|
| 87 | + const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object); |
| 88 | + const auto minimum_lane_change_length = calcMinimumLaneChangeLength(); |
88 | 89 |
|
89 |
| - // get minimum lane change distance |
90 |
| - const auto shift_intervals = |
91 |
| - getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); |
92 |
| - const double minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( |
93 |
| - *lane_change_parameters_, shift_intervals, |
94 |
| - lane_change_parameters_->backward_length_buffer_for_end_of_lane); |
95 |
| - |
96 |
| - // get minimum avoid distance |
97 |
| - |
98 |
| - const auto ego_width = getCommonParam().vehicle_width; |
99 |
| - const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); |
100 |
| - const auto nearest_object_parameter = |
101 |
| - avoidance_parameters_->object_parameters.at(nearest_object_type); |
102 |
| - const auto avoid_margin = |
103 |
| - nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + |
104 |
| - nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; |
105 |
| - |
106 |
| - avoidance_helper_->setData(planner_data_); |
107 |
| - const auto shift_length = avoidance_helper_->getShiftLength( |
108 |
| - nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); |
109 |
| - |
110 |
| - const auto maximum_avoid_distance = avoidance_helper_->getMaxAvoidanceDistance(shift_length); |
111 |
| - |
112 |
| - RCLCPP_DEBUG( |
113 |
| - logger_, |
114 |
| - "nearest_object.longitudinal %.3f, minimum_lane_change_length %.3f, maximum_avoid_distance " |
115 |
| - "%.3f", |
116 |
| - nearest_object.longitudinal, minimum_lane_change_length, maximum_avoid_distance); |
| 90 | + lane_change_debug_.execution_area = createExecutionArea( |
| 91 | + getCommonParam().vehicle_info, getEgoPose(), |
| 92 | + std::max(minimum_lane_change_length, minimum_avoid_length), calcLateralOffset()); |
117 | 93 |
|
118 |
| - return nearest_object.longitudinal > std::max(minimum_lane_change_length, maximum_avoid_distance); |
| 94 | + return nearest_object.longitudinal > std::max(minimum_lane_change_length, minimum_avoid_length); |
119 | 95 | }
|
120 | 96 |
|
121 | 97 | bool AvoidanceByLaneChange::specialExpiredCheck() const
|
@@ -284,4 +260,48 @@ ObjectData AvoidanceByLaneChange::createObjectData(
|
284 | 260 |
|
285 | 261 | return object_data;
|
286 | 262 | }
|
| 263 | + |
| 264 | +double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_object) const |
| 265 | +{ |
| 266 | + const auto ego_width = getCommonParam().vehicle_width; |
| 267 | + const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification); |
| 268 | + const auto nearest_object_parameter = |
| 269 | + avoidance_parameters_->object_parameters.at(nearest_object_type); |
| 270 | + const auto avoid_margin = |
| 271 | + nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor + |
| 272 | + nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width; |
| 273 | + |
| 274 | + avoidance_helper_->setData(planner_data_); |
| 275 | + const auto shift_length = avoidance_helper_->getShiftLength( |
| 276 | + nearest_object, utils::avoidance::isOnRight(nearest_object), avoid_margin); |
| 277 | + |
| 278 | + return avoidance_helper_->getMinAvoidanceDistance(shift_length); |
| 279 | +} |
| 280 | + |
| 281 | +double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const |
| 282 | +{ |
| 283 | + const auto current_lanes = getCurrentLanes(); |
| 284 | + if (current_lanes.empty()) { |
| 285 | + RCLCPP_DEBUG(logger_, "no empty lanes"); |
| 286 | + return std::numeric_limits<double>::infinity(); |
| 287 | + } |
| 288 | + |
| 289 | + // get minimum lane change distance |
| 290 | + const auto shift_intervals = |
| 291 | + getRouteHandler()->getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); |
| 292 | + return utils::lane_change::calcMinimumLaneChangeLength( |
| 293 | + *lane_change_parameters_, shift_intervals, |
| 294 | + lane_change_parameters_->backward_length_buffer_for_end_of_lane); |
| 295 | +} |
| 296 | + |
| 297 | +double AvoidanceByLaneChange::calcLateralOffset() const |
| 298 | +{ |
| 299 | + auto additional_lat_offset{0.0}; |
| 300 | + for (const auto & [type, p] : avoidance_parameters_->object_parameters) { |
| 301 | + const auto offset = |
| 302 | + 2.0 * p.envelope_buffer_margin + p.safety_buffer_lateral + p.avoid_margin_lateral; |
| 303 | + additional_lat_offset = std::max(additional_lat_offset, offset); |
| 304 | + } |
| 305 | + return additional_lat_offset; |
| 306 | +} |
287 | 307 | } // namespace behavior_path_planner
|
0 commit comments