Skip to content

Commit 2bccaa1

Browse files
satoshi-otasatoshi-ota
committed
fix(avoidance): the module ignored merging objects unexpectedly (autowarefoundation#6601)
* feat(avoidance): output overhang lanelet Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): avoid merging vehicle if it's NOT on ego lane. Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): add flag to identify ambiguous vehicle Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(avoidance): add helper function Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(avoidance): rename param Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * chore(avoidance): update comment Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent dc15ad5 commit 2bccaa1

File tree

9 files changed

+96
-60
lines changed

9 files changed

+96
-60
lines changed

‎planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -132,7 +132,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
132132
}
133133

134134
{
135-
const std::string ns = "avoidance.target_filtering.force_avoidance.";
135+
const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
136136
p.enable_force_avoidance_for_stopped_vehicle =
137137
getOrDeclareParameter<bool>(*node, ns + "enable");
138138
p.threshold_time_force_avoidance_for_stopped_vehicle =

‎planning/behavior_path_avoidance_module/config/avoidance.param.yaml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -156,7 +156,7 @@
156156
backward_distance: 10.0 # [m]
157157

158158
# params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
159-
force_avoidance:
159+
avoidance_for_ambiguous_vehicle:
160160
enable: false # [-]
161161
time_threshold: 1.0 # [s]
162162
distance_threshold: 1.0 # [m]

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -384,7 +384,8 @@ struct ObjectData // avoidance target
384384
rclcpp::Time last_move;
385385
double stop_time{0.0};
386386

387-
// store the information of the lanelet which the object's overhang is currently occupying
387+
// It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to
388+
// check whether the object is on the ego lane.
388389
lanelet::ConstLanelet overhang_lanelet;
389390

390391
// the position at the detected moment
@@ -420,6 +421,12 @@ struct ObjectData // avoidance target
420421
// is parked vehicle on road shoulder
421422
bool is_parked{false};
422423

424+
// is driving on ego current lane
425+
bool is_on_ego_lane{false};
426+
427+
// is ambiguous stopped vehicle.
428+
bool is_ambiguous{false};
429+
423430
// object direction.
424431
Direction direction{Direction::NONE};
425432

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717

1818
#include "behavior_path_avoidance_module/data_structs.hpp"
1919
#include "behavior_path_avoidance_module/utils.hpp"
20+
#include "behavior_path_planner_common/utils/utils.hpp"
2021

2122
#include <motion_utils/distance/distance.hpp>
2223

@@ -134,6 +135,24 @@ class AvoidanceHelper
134135
shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed());
135136
}
136137

138+
double getFrontConstantDistance(const ObjectData & object) const
139+
{
140+
const auto object_type = utils::getHighestProbLabel(object.object.classification);
141+
const auto object_parameter = parameters_->object_parameters.at(object_type);
142+
const auto & additional_buffer_longitudinal =
143+
object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
144+
: 0.0;
145+
return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
146+
}
147+
148+
double getRearConstantDistance(const ObjectData & object) const
149+
{
150+
const auto object_type = utils::getHighestProbLabel(object.object.classification);
151+
const auto object_parameter = parameters_->object_parameters.at(object_type);
152+
return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear +
153+
object.length;
154+
}
155+
137156
double getEgoShift() const
138157
{
139158
validate();

‎planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -140,7 +140,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
140140
}
141141

142142
{
143-
const std::string ns = "avoidance.target_filtering.force_avoidance.";
143+
const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
144144
p.enable_force_avoidance_for_stopped_vehicle =
145145
getOrDeclareParameter<bool>(*node, ns + "enable");
146146
p.threshold_time_force_avoidance_for_stopped_vehicle =

‎planning/behavior_path_avoidance_module/src/debug.cpp

Lines changed: 23 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -188,10 +188,25 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
188188
return msg;
189189
}
190190

191+
MarkerArray createOverhangLaneletMarkerArray(const ObjectDataArray & objects, std::string && ns)
192+
{
193+
MarkerArray msg;
194+
msg.markers.reserve(objects.size());
195+
196+
for (const auto & object : objects) {
197+
appendMarkerArray(
198+
marker_utils::createLaneletsAreaMarkerArray(
199+
{object.overhang_lanelet}, std::string(ns), 0.0, 0.0, 1.0),
200+
&msg);
201+
}
202+
203+
return msg;
204+
}
205+
191206
MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
192207
{
193208
MarkerArray msg;
194-
msg.markers.reserve(objects.size() * 4);
209+
msg.markers.reserve(objects.size() * 5);
195210

196211
appendMarkerArray(
197212
createObjectsCubeMarkerArray(
@@ -202,14 +217,15 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st
202217
appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
203218
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
204219
appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
220+
appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
205221

206222
return msg;
207223
}
208224

209225
MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::string && ns)
210226
{
211227
MarkerArray msg;
212-
msg.markers.reserve(objects.size() * 4);
228+
msg.markers.reserve(objects.size() * 5);
213229

214230
appendMarkerArray(
215231
createObjectsCubeMarkerArray(
@@ -220,6 +236,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::
220236
appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
221237
appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
222238
appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
239+
appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
223240

224241
return msg;
225242
}
@@ -451,6 +468,10 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
451468
appendMarkerArray(
452469
createObjectInfoMarkerArray(filtered_objects, "others_" + convertToSnakeCase(ns) + "_info"),
453470
&msg);
471+
appendMarkerArray(
472+
createOverhangLaneletMarkerArray(
473+
filtered_objects, "others_" + convertToSnakeCase(ns) + "_overhang_lanelet"),
474+
&msg);
454475

455476
return msg;
456477
}

‎planning/behavior_path_avoidance_module/src/scene.cpp

Lines changed: 11 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -638,21 +638,16 @@ void AvoidanceModule::fillDebugData(
638638
const auto o_front = data.stop_target_object.value();
639639
const auto object_type = utils::getHighestProbLabel(o_front.object.classification);
640640
const auto object_parameter = parameters_->object_parameters.at(object_type);
641-
const auto & additional_buffer_longitudinal =
642-
object_parameter.use_conservative_buffer_longitudinal
643-
? planner_data_->parameters.base_link2front
644-
: 0.0;
641+
const auto constant_distance = helper_->getFrontConstantDistance(o_front);
645642
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
646643

647644
const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor +
648645
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
649646

650-
const auto variable = helper_->getSharpAvoidanceDistance(
647+
const auto avoidance_distance = helper_->getSharpAvoidanceDistance(
651648
helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
652-
const auto constant = helper_->getNominalPrepareDistance() +
653-
object_parameter.safety_buffer_longitudinal +
654-
additional_buffer_longitudinal;
655-
const auto total_avoid_distance = variable + constant;
649+
const auto prepare_distance = helper_->getNominalPrepareDistance();
650+
const auto total_avoid_distance = prepare_distance + avoidance_distance + constant_distance;
656651

657652
dead_pose_ = calcLongitudinalOffsetPose(
658653
data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance);
@@ -1419,16 +1414,14 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
14191414

14201415
const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
14211416
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
1422-
const auto variable = helper_->getMinAvoidanceDistance(
1417+
const auto avoidance_distance = helper_->getMinAvoidanceDistance(
14231418
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
1424-
const auto & additional_buffer_longitudinal =
1425-
object_parameter.use_conservative_buffer_longitudinal
1426-
? planner_data_->parameters.base_link2front
1427-
: 0.0;
1428-
const auto constant = p->min_prepare_distance + object_parameter.safety_buffer_longitudinal +
1429-
additional_buffer_longitudinal + p->stop_buffer;
1430-
1431-
return object.longitudinal - std::min(variable + constant, p->stop_max_distance);
1419+
const auto constant_distance = helper_->getFrontConstantDistance(object);
1420+
1421+
return object.longitudinal -
1422+
std::min(
1423+
avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer,
1424+
p->stop_max_distance);
14321425
}
14331426

14341427
void AvoidanceModule::insertReturnDeadLine(

‎planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

Lines changed: 9 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -123,7 +123,6 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
123123
{
124124
// To be consistent with changes in the ego position, the current shift length is considered.
125125
const auto current_ego_shift = helper_->getEgoShift();
126-
const auto & base_link2rear = data_->parameters.base_link2rear;
127126

128127
// Calculate feasible shift length
129128
const auto get_shift_profile =
@@ -140,13 +139,10 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
140139

141140
// calculate remaining distance.
142141
const auto prepare_distance = helper_->getNominalPrepareDistance();
143-
const auto & additional_buffer_longitudinal =
144-
object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
145-
: 0.0;
146-
const auto constant = object_parameter.safety_buffer_longitudinal +
147-
additional_buffer_longitudinal + prepare_distance;
148-
const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance;
149-
const auto remaining_distance = object.longitudinal - constant;
142+
const auto constant_distance = helper_->getFrontConstantDistance(object);
143+
const auto has_enough_distance =
144+
object.longitudinal > constant_distance + prepare_distance + nominal_avoid_distance;
145+
const auto remaining_distance = object.longitudinal - constant_distance - prepare_distance;
150146
const auto avoidance_distance =
151147
has_enough_distance ? nominal_avoid_distance : remaining_distance;
152148

@@ -278,11 +274,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
278274
}
279275
}
280276

281-
// use each object param
282-
const auto object_type = utils::getHighestProbLabel(o.object.classification);
283-
const auto object_parameter = parameters_->object_parameters.at(object_type);
277+
// calculate feasible shift length based on behavior policy
284278
const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
285-
286279
if (!feasible_shift_profile.has_value()) {
287280
if (o.avoid_required && is_forward_object(o)) {
288281
break;
@@ -297,12 +290,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
297290

298291
AvoidLine al_avoid;
299292
{
300-
const auto & additional_buffer_longitudinal =
301-
object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
302-
: 0.0;
303-
const auto offset =
304-
object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
305-
const auto to_shift_end = o.longitudinal - offset;
293+
const auto constant_distance = helper_->getFrontConstantDistance(o);
294+
const auto to_shift_end = o.longitudinal - constant_distance;
306295
const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index);
307296

308297
// start point (use previous linear shift length as start shift length.)
@@ -338,8 +327,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
338327

339328
AvoidLine al_return;
340329
{
341-
const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length;
342-
const auto to_shift_start = o.longitudinal + offset;
330+
const auto constant_distance = helper_->getRearConstantDistance(o);
331+
const auto to_shift_start = o.longitudinal + constant_distance;
343332

344333
// start point
345334
al_return.start_shift_length = feasible_shift_profile.value().first;

‎planning/behavior_path_avoidance_module/src/utils.cpp

Lines changed: 23 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -444,6 +444,14 @@ bool isWithinIntersection(
444444
object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
445445
}
446446

447+
bool isOnEgoLane(const ObjectData & object)
448+
{
449+
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
450+
return boost::geometry::within(
451+
lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
452+
object.overhang_lanelet.polygon2d().basicPolygon());
453+
}
454+
447455
bool isParallelToEgoLane(const ObjectData & object, const double threshold)
448456
{
449457
const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
@@ -491,6 +499,10 @@ bool isParkedVehicle(
491499
using lanelet::utils::to2D;
492500
using lanelet::utils::conversion::toLaneletPoint;
493501

502+
if (object.is_within_intersection) {
503+
return false;
504+
}
505+
494506
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
495507
const auto centerline_pos =
496508
lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position;
@@ -579,15 +591,11 @@ bool isParkedVehicle(
579591
return is_left_side_parked_vehicle || is_right_side_parked_vehicle;
580592
}
581593

582-
bool isForceAvoidanceTarget(
594+
bool isAmbiguousStoppedVehicle(
583595
ObjectData & object, const AvoidancePlanningData & data,
584596
const std::shared_ptr<const PlannerData> & planner_data,
585597
const std::shared_ptr<AvoidanceParameters> & parameters)
586598
{
587-
if (!parameters->enable_force_avoidance_for_stopped_vehicle) {
588-
return false;
589-
}
590-
591599
const auto stop_time_longer_than_threshold =
592600
object.stop_time > parameters->threshold_time_force_avoidance_for_stopped_vehicle;
593601

@@ -618,6 +626,10 @@ bool isForceAvoidanceTarget(
618626
return false;
619627
}
620628

629+
if (!object.is_on_ego_lane) {
630+
return true;
631+
}
632+
621633
const auto & ego_pose = planner_data->self_odometry->pose.pose;
622634

623635
// force avoidance for stopped vehicle
@@ -744,24 +756,17 @@ bool isSatisfiedWithVehicleCondition(
744756
const std::shared_ptr<const PlannerData> & planner_data,
745757
const std::shared_ptr<AvoidanceParameters> & parameters)
746758
{
747-
using boost::geometry::within;
748-
using lanelet::utils::to2D;
749-
using lanelet::utils::conversion::toLaneletPoint;
750-
751759
object.behavior = getObjectBehavior(object, parameters);
752-
object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler);
760+
object.is_on_ego_lane = isOnEgoLane(object);
761+
object.is_ambiguous = isAmbiguousStoppedVehicle(object, data, planner_data, parameters);
753762

754763
// from here condition check for vehicle type objects.
755-
if (isForceAvoidanceTarget(object, data, planner_data, parameters)) {
764+
if (object.is_ambiguous && parameters->enable_force_avoidance_for_stopped_vehicle) {
756765
return true;
757766
}
758767

759768
// check vehicle shift ratio
760-
const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
761-
const auto on_ego_driving_lane = within(
762-
to2D(toLaneletPoint(object_pos)).basicPoint(),
763-
object.overhang_lanelet.polygon2d().basicPolygon());
764-
if (on_ego_driving_lane) {
769+
if (object.is_on_ego_lane) {
765770
if (object.is_parked) {
766771
return true;
767772
} else {
@@ -1721,6 +1726,8 @@ void filterTargetObjects(
17211726
}
17221727

17231728
if (filtering_utils::isVehicleTypeObject(o)) {
1729+
o.is_within_intersection =
1730+
filtering_utils::isWithinIntersection(o, planner_data->route_handler);
17241731
o.is_parked = filtering_utils::isParkedVehicle(o, planner_data->route_handler, parameters);
17251732
o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters);
17261733

0 commit comments

Comments
 (0)