Skip to content

Commit 8c1398d

Browse files
zulfaqar-azmi-t4mkquda
authored and
kyoichi-sugahara
committed
refactor(lane_change): separate target lane leading based on obj behavior (autowarefoundation#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 def11e8 commit 8c1398d

File tree

12 files changed

+283
-305
lines changed

12 files changed

+283
-305
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
@@ -127,16 +127,13 @@ class NormalLaneChange : public LaneChangeBase
127127
std::vector<double> calc_prepare_durations() const;
128128

129129
lane_change::TargetObjects get_target_objects(
130-
const FilteredByLanesExtendedObjects & filtered_objects,
130+
const FilteredLanesObjects & filtered_objects,
131131
const lanelet::ConstLanelets & current_lanes) const;
132132

133-
FilteredByLanesExtendedObjects filterObjects() const;
133+
FilteredLanesObjects filter_objects() const;
134134

135135
void filterOncomingObjects(PredictedObjects & objects) const;
136136

137-
FilteredByLanesObjects filterObjectsByLanelets(
138-
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;
139-
140137
bool get_prepare_segment(
141138
PathWithLaneId & prepare_segment, const double prepare_length) const override;
142139

‎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
@@ -275,7 +275,7 @@ class LaneChangeBase
275275
std::shared_ptr<LaneChangePath> abort_path_{};
276276
std::shared_ptr<const PlannerData> planner_data_{};
277277
lane_change::CommonDataPtr common_data_ptr_{};
278-
FilteredByLanesExtendedObjects filtered_objects_{};
278+
FilteredLanesObjects filtered_objects_{};
279279
BehaviorModuleOutput prev_module_output_{};
280280
std::optional<Pose> lane_change_stop_pose_{std::nullopt};
281281

‎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;
@@ -416,8 +419,7 @@ using LaneChangeStates = lane_change::States;
416419
using LaneChangePhaseInfo = lane_change::PhaseInfo;
417420
using LaneChangePhaseMetrics = lane_change::PhaseMetrics;
418421
using LaneChangeInfo = lane_change::Info;
419-
using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
420-
using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;
422+
using FilteredLanesObjects = lane_change::FilteredLanesObjects;
421423
using LateralAccelerationMap = lane_change::LateralAccelerationMap;
422424
} // namespace autoware::behavior_path_planner
423425

‎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;
@@ -154,8 +155,7 @@ lanelet::BasicPolygon2d create_polygon(
154155
const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);
155156

156157
ExtendedPredictedObject transform(
157-
const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
158-
const LaneChangeParameters & lane_change_parameters);
158+
const PredictedObject & object, const LaneChangeParameters & lane_change_parameters);
159159

160160
bool is_collided_polygons_in_lanelet(
161161
const std::vector<Polygon2d> & collided_polygons, const lanelet::BasicPolygon2d & lanes_polygon);
@@ -254,17 +254,14 @@ Pose to_pose(
254254

255255
bool is_ahead_of_ego(
256256
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
257-
const PredictedObject & object);
257+
const ExtendedPredictedObject & object);
258258

259259
bool is_before_terminal(
260260
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
261-
const PredictedObject & object);
261+
const ExtendedPredictedObject & object);
262262

263263
double calc_angle_to_lanelet_segment(const lanelet::ConstLanelets & lanelets, const Pose & pose);
264264

265-
ExtendedPredictedObjects transform_to_extended_objects(
266-
const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects);
267-
268265
double get_distance_to_next_regulatory_element(
269266
const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,
270267
const bool ignore_intersection = false);
@@ -288,7 +285,7 @@ double get_distance_to_next_regulatory_element(
288285
* found, returns the maximum possible double value.
289286
*/
290287
double get_min_dist_to_current_lanes_obj(
291-
const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
288+
const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects,
292289
const double dist_to_target_lane_start, const PathWithLaneId & path);
293290

294291
/**
@@ -308,8 +305,8 @@ double get_min_dist_to_current_lanes_obj(
308305
* otherwise, false.
309306
*/
310307
bool has_blocking_target_object(
311-
const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
312-
const double stop_arc_length, const PathWithLaneId & path);
308+
const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length,
309+
const PathWithLaneId & path);
313310

314311
/**
315312
* @brief Checks if the ego vehicle has passed any turn direction within an intersection.
@@ -356,6 +353,35 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
356353
*/
357354
bool has_overtaking_turn_lane_object(
358355
const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects);
356+
357+
/**
358+
* @brief Filters objects based on their positions and velocities relative to the ego vehicle and
359+
* the target lane.
360+
*
361+
* This function evaluates whether an object should be classified as a leading or trailing object
362+
* in the context of a lane change. Objects are filtered based on their lateral distance from
363+
* the ego vehicle, velocity, and whether they are within the target lane or its expanded
364+
* boundaries.
365+
*
366+
* @param common_data_ptr Shared pointer to CommonData containing information about current lanes,
367+
* vehicle dimensions, lane polygons, and behavior parameters.
368+
* @param object An extended predicted object representing a potential obstacle in the environment.
369+
* @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the
370+
* current lanes.
371+
* @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle.
372+
* @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of
373+
* the lane.
374+
* @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or
375+
* outside boundaries).
376+
* @param trailing_objects Reference to a collection for storing trailing objects.
377+
*
378+
* @return true if the object is classified as either leading or trailing, false otherwise.
379+
*/
380+
bool filter_target_lane_objects(
381+
const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object,
382+
const double dist_ego_to_current_lanes_center, const bool ahead_of_ego,
383+
const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
384+
ExtendedPredictedObjects & trailing_objects);
359385
} // namespace autoware::behavior_path_planner::utils::lane_change
360386

361387
namespace autoware::behavior_path_planner::utils::lane_change::debug

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
<depend>autoware_rtc_interface</depend>
2727
<depend>autoware_universe_utils</depend>
2828
<depend>pluginlib</depend>
29+
<depend>range-v3</depend>
2930
<depend>rclcpp</depend>
3031
<depend>tier4_planning_msgs</depend>
3132
<depend>visualization_msgs</depend>

0 commit comments

Comments
 (0)