Skip to content

Commit 25e7d5d

Browse files
zulfaqar-azmi-t4mkquda
zulfaqar-azmi-t4
and
mkquda
authored
refactor(lane_change): separate target lane leading based on obj behavior (#9372)
* separate target lane leading objects based on behavior (RT1-8532) Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fixed overlapped filtering and lanes debug marker Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * combine filteredObjects function Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * renaming functions and type Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * update some logic to check is stopped Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * rename expanded to stopped_outside_boundary Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Include docstring Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * rename stopped_outside_boundary → stopped_at_bound Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * spell-check Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com>
1 parent e5181a5 commit 25e7d5d

File tree

13 files changed

+284
-313
lines changed

13 files changed

+284
-313
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

+2-5
Original file line numberDiff line numberDiff line change
@@ -121,16 +121,13 @@ class NormalLaneChange : public LaneChangeBase
121121
TurnSignalInfo get_terminal_turn_signal_info() const final;
122122

123123
lane_change::TargetObjects get_target_objects(
124-
const FilteredByLanesExtendedObjects & filtered_objects,
124+
const FilteredLanesObjects & filtered_objects,
125125
const lanelet::ConstLanelets & current_lanes) const;
126126

127-
FilteredByLanesExtendedObjects filterObjects() const;
127+
FilteredLanesObjects filter_objects() const;
128128

129129
void filterOncomingObjects(PredictedObjects & objects) const;
130130

131-
FilteredByLanesObjects filterObjectsByLanelets(
132-
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;
133-
134131
bool get_prepare_segment(
135132
PathWithLaneId & prepare_segment, const double prepare_length) const override;
136133

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -276,7 +276,7 @@ class LaneChangeBase
276276
std::shared_ptr<LaneChangePath> abort_path_{};
277277
std::shared_ptr<const PlannerData> planner_data_{};
278278
lane_change::CommonDataPtr common_data_ptr_{};
279-
FilteredByLanesExtendedObjects filtered_objects_{};
279+
FilteredLanesObjects filtered_objects_{};
280280
BehaviorModuleOutput prev_module_output_{};
281281
std::optional<Pose> lane_change_stop_pose_{std::nullopt};
282282

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

+18-16
Original file line numberDiff line numberDiff line change
@@ -262,25 +262,28 @@ struct Info
262262
}
263263
};
264264

265-
template <typename Object>
266-
struct LanesObjects
265+
struct TargetLaneLeadingObjects
267266
{
268-
Object current_lane{};
269-
Object target_lane_leading{};
270-
Object target_lane_trailing{};
271-
Object other_lane{};
272-
273-
LanesObjects() = default;
274-
LanesObjects(
275-
Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane)
276-
: current_lane(std::move(current_lane)),
277-
target_lane_leading(std::move(target_lane_leading)),
278-
target_lane_trailing(std::move(target_lane_trailing)),
279-
other_lane(std::move(other_lane))
267+
ExtendedPredictedObjects moving;
268+
ExtendedPredictedObjects stopped;
269+
270+
// for objects outside of target lanes, but close to its boundaries
271+
ExtendedPredictedObjects stopped_at_bound;
272+
273+
[[nodiscard]] size_t size() const
280274
{
275+
return moving.size() + stopped.size() + stopped_at_bound.size();
281276
}
282277
};
283278

279+
struct FilteredLanesObjects
280+
{
281+
ExtendedPredictedObjects others;
282+
ExtendedPredictedObjects current_lane;
283+
ExtendedPredictedObjects target_lane_trailing;
284+
TargetLaneLeadingObjects target_lane_leading;
285+
};
286+
284287
struct TargetObjects
285288
{
286289
ExtendedPredictedObjects leading;
@@ -418,8 +421,7 @@ using LaneChangeStates = lane_change::States;
418421
using LaneChangePhaseInfo = lane_change::PhaseInfo;
419422
using LaneChangePhaseMetrics = lane_change::PhaseMetrics;
420423
using LaneChangeInfo = lane_change::Info;
421-
using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
422-
using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;
424+
using FilteredLanesObjects = lane_change::FilteredLanesObjects;
423425
using LateralAccelerationMap = lane_change::LateralAccelerationMap;
424426
} // namespace autoware::behavior_path_planner
425427

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ struct Debug
3535
LaneChangePaths valid_paths;
3636
CollisionCheckDebugMap collision_check_objects;
3737
CollisionCheckDebugMap collision_check_objects_after_approval;
38-
FilteredByLanesExtendedObjects filtered_objects;
38+
FilteredLanesObjects filtered_objects;
3939
geometry_msgs::msg::Polygon execution_area;
4040
geometry_msgs::msg::Pose ego_pose;
4141
lanelet::ConstLanelets current_lanes;
@@ -55,9 +55,11 @@ struct Debug
5555
collision_check_objects.clear();
5656
collision_check_objects_after_approval.clear();
5757
filtered_objects.current_lane.clear();
58-
filtered_objects.target_lane_leading.clear();
5958
filtered_objects.target_lane_trailing.clear();
60-
filtered_objects.other_lane.clear();
59+
filtered_objects.target_lane_leading.moving.clear();
60+
filtered_objects.target_lane_leading.stopped.clear();
61+
filtered_objects.target_lane_leading.stopped_at_bound.clear();
62+
filtered_objects.others.clear();
6163
execution_area.points.clear();
6264
current_lanes.clear();
6365
target_lanes.clear();

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@
2929

3030
namespace marker_utils::lane_change_markers
3131
{
32-
using autoware::behavior_path_planner::FilteredByLanesExtendedObjects;
32+
using autoware::behavior_path_planner::FilteredLanesObjects;
3333
using autoware::behavior_path_planner::LaneChangePath;
3434
using autoware::behavior_path_planner::lane_change::Debug;
3535
using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
@@ -40,7 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker(
4040
const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
4141
const rclcpp::Time & now, const std::string & ns);
4242
MarkerArray showFilteredObjects(
43-
const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns);
43+
const FilteredLanesObjects & filtered_objects, const std::string & ns);
4444
MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
4545
MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
4646
MarkerArray createDebugMarkerArray(

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

+36-10
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,7 @@ using behavior_path_planner::lane_change::CommonDataPtr;
5454
using behavior_path_planner::lane_change::LanesPolygon;
5555
using behavior_path_planner::lane_change::ModuleType;
5656
using behavior_path_planner::lane_change::PathSafetyStatus;
57+
using behavior_path_planner::lane_change::TargetLaneLeadingObjects;
5758
using geometry_msgs::msg::Point;
5859
using geometry_msgs::msg::Pose;
5960
using geometry_msgs::msg::Twist;
@@ -144,8 +145,7 @@ lanelet::BasicPolygon2d create_polygon(
144145
const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);
145146

146147
ExtendedPredictedObject transform(
147-
const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
148-
const LaneChangeParameters & lane_change_parameters);
148+
const PredictedObject & object, const LaneChangeParameters & lane_change_parameters);
149149

150150
bool is_collided_polygons_in_lanelet(
151151
const std::vector<Polygon2d> & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon);
@@ -240,17 +240,14 @@ bool is_same_lane_with_prev_iteration(
240240

241241
bool is_ahead_of_ego(
242242
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
243-
const PredictedObject & object);
243+
const ExtendedPredictedObject & object);
244244

245245
bool is_before_terminal(
246246
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
247-
const PredictedObject & object);
247+
const ExtendedPredictedObject & object);
248248

249249
double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose);
250250

251-
ExtendedPredictedObjects transform_to_extended_objects(
252-
const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects);
253-
254251
double get_distance_to_next_regulatory_element(
255252
const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,
256253
const bool ignore_intersection = false);
@@ -274,7 +271,7 @@ double get_distance_to_next_regulatory_element(
274271
* found, returns the maximum possible double value.
275272
*/
276273
double get_min_dist_to_current_lanes_obj(
277-
const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
274+
const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects,
278275
const double dist_to_target_lane_start, const PathWithLaneId & path);
279276

280277
/**
@@ -294,8 +291,8 @@ double get_min_dist_to_current_lanes_obj(
294291
* otherwise, false.
295292
*/
296293
bool has_blocking_target_object(
297-
const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
298-
const double stop_arc_length, const PathWithLaneId & path);
294+
const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length,
295+
const PathWithLaneId & path);
299296

300297
/**
301298
* @brief Checks if the ego vehicle has passed any turn direction within an intersection.
@@ -342,5 +339,34 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
342339
*/
343340
bool has_overtaking_turn_lane_object(
344341
const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects);
342+
343+
/**
344+
* @brief Filters objects based on their positions and velocities relative to the ego vehicle and
345+
* the target lane.
346+
*
347+
* This function evaluates whether an object should be classified as a leading or trailing object
348+
* in the context of a lane change. Objects are filtered based on their lateral distance from
349+
* the ego vehicle, velocity, and whether they are within the target lane or its expanded
350+
* boundaries.
351+
*
352+
* @param common_data_ptr Shared pointer to CommonData containing information about current lanes,
353+
* vehicle dimensions, lane polygons, and behavior parameters.
354+
* @param object An extended predicted object representing a potential obstacle in the environment.
355+
* @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the
356+
* current lanes.
357+
* @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle.
358+
* @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of
359+
* the lane.
360+
* @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or
361+
* outside boundaries).
362+
* @param trailing_objects Reference to a collection for storing trailing objects.
363+
*
364+
* @return true if the object is classified as either leading or trailing, false otherwise.
365+
*/
366+
bool filter_target_lane_objects(
367+
const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object,
368+
const double dist_ego_to_current_lanes_center, const bool ahead_of_ego,
369+
const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
370+
ExtendedPredictedObjects & trailing_objects);
345371
} // namespace autoware::behavior_path_planner::utils::lane_change
346372
#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,7 @@
2727
<depend>autoware_rtc_interface</depend>
2828
<depend>autoware_universe_utils</depend>
2929
<depend>pluginlib</depend>
30+
<depend>range-v3</depend>
3031
<depend>rclcpp</depend>
3132
<depend>tier4_planning_msgs</depend>
3233
<depend>visualization_msgs</depend>

0 commit comments

Comments
 (0)