Skip to content

Commit

Permalink
refactor(lane_change): separate target lane leading based on obj beha…
Browse files Browse the repository at this point in the history
…vior (#9372)

* separate target lane leading objects based on behavior (RT1-8532)

Signed-off-by: Zulfaqar Azmi <[email protected]>

* fixed overlapped filtering and lanes debug marker

Signed-off-by: Zulfaqar Azmi <[email protected]>

* combine filteredObjects function

Signed-off-by: Zulfaqar Azmi <[email protected]>

* renaming functions and type

Signed-off-by: Zulfaqar Azmi <[email protected]>

* update some logic to check is stopped

Signed-off-by: Zulfaqar Azmi <[email protected]>

* rename expanded to stopped_outside_boundary

Signed-off-by: Zulfaqar Azmi <[email protected]>

* Include docstring

Signed-off-by: Zulfaqar Azmi <[email protected]>

* rename stopped_outside_boundary → stopped_at_bound

Signed-off-by: Zulfaqar Azmi <[email protected]>

* 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 <[email protected]>
Signed-off-by: Zulfaqar Azmi <[email protected]>

* 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 <[email protected]>
Signed-off-by: Zulfaqar Azmi <[email protected]>

* spell-check

Signed-off-by: Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Zulfaqar Azmi <[email protected]>
Co-authored-by: mkquda <[email protected]>
  • Loading branch information
zulfaqar-azmi-t4 and mkquda authored Nov 22, 2024
1 parent e5181a5 commit 25e7d5d
Show file tree
Hide file tree
Showing 13 changed files with 284 additions and 313 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -121,16 +121,13 @@ class NormalLaneChange : public LaneChangeBase
TurnSignalInfo get_terminal_turn_signal_info() const final;

lane_change::TargetObjects get_target_objects(
const FilteredByLanesExtendedObjects & filtered_objects,
const FilteredLanesObjects & filtered_objects,
const lanelet::ConstLanelets & current_lanes) const;

FilteredByLanesExtendedObjects filterObjects() const;
FilteredLanesObjects filter_objects() const;

void filterOncomingObjects(PredictedObjects & objects) const;

FilteredByLanesObjects filterObjectsByLanelets(
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;

bool get_prepare_segment(
PathWithLaneId & prepare_segment, const double prepare_length) const override;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -276,7 +276,7 @@ class LaneChangeBase
std::shared_ptr<LaneChangePath> abort_path_{};
std::shared_ptr<const PlannerData> planner_data_{};
lane_change::CommonDataPtr common_data_ptr_{};
FilteredByLanesExtendedObjects filtered_objects_{};
FilteredLanesObjects filtered_objects_{};
BehaviorModuleOutput prev_module_output_{};
std::optional<Pose> lane_change_stop_pose_{std::nullopt};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -262,25 +262,28 @@ struct Info
}
};

template <typename Object>
struct LanesObjects
struct TargetLaneLeadingObjects
{
Object current_lane{};
Object target_lane_leading{};
Object target_lane_trailing{};
Object other_lane{};

LanesObjects() = default;
LanesObjects(
Object current_lane, Object target_lane_leading, Object target_lane_trailing, Object other_lane)
: current_lane(std::move(current_lane)),
target_lane_leading(std::move(target_lane_leading)),
target_lane_trailing(std::move(target_lane_trailing)),
other_lane(std::move(other_lane))
ExtendedPredictedObjects moving;
ExtendedPredictedObjects stopped;

// for objects outside of target lanes, but close to its boundaries
ExtendedPredictedObjects stopped_at_bound;

[[nodiscard]] size_t size() const
{
return moving.size() + stopped.size() + stopped_at_bound.size();
}
};

struct FilteredLanesObjects
{
ExtendedPredictedObjects others;
ExtendedPredictedObjects current_lane;
ExtendedPredictedObjects target_lane_trailing;
TargetLaneLeadingObjects target_lane_leading;
};

struct TargetObjects
{
ExtendedPredictedObjects leading;
Expand Down Expand Up @@ -418,8 +421,7 @@ using LaneChangeStates = lane_change::States;
using LaneChangePhaseInfo = lane_change::PhaseInfo;
using LaneChangePhaseMetrics = lane_change::PhaseMetrics;
using LaneChangeInfo = lane_change::Info;
using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;
using FilteredLanesObjects = lane_change::FilteredLanesObjects;
using LateralAccelerationMap = lane_change::LateralAccelerationMap;
} // namespace autoware::behavior_path_planner

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ struct Debug
LaneChangePaths valid_paths;
CollisionCheckDebugMap collision_check_objects;
CollisionCheckDebugMap collision_check_objects_after_approval;
FilteredByLanesExtendedObjects filtered_objects;
FilteredLanesObjects filtered_objects;
geometry_msgs::msg::Polygon execution_area;
geometry_msgs::msg::Pose ego_pose;
lanelet::ConstLanelets current_lanes;
Expand All @@ -55,9 +55,11 @@ struct Debug
collision_check_objects.clear();
collision_check_objects_after_approval.clear();
filtered_objects.current_lane.clear();
filtered_objects.target_lane_leading.clear();
filtered_objects.target_lane_trailing.clear();
filtered_objects.other_lane.clear();
filtered_objects.target_lane_leading.moving.clear();
filtered_objects.target_lane_leading.stopped.clear();
filtered_objects.target_lane_leading.stopped_at_bound.clear();
filtered_objects.others.clear();
execution_area.points.clear();
current_lanes.clear();
target_lanes.clear();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@

namespace marker_utils::lane_change_markers
{
using autoware::behavior_path_planner::FilteredByLanesExtendedObjects;
using autoware::behavior_path_planner::FilteredLanesObjects;
using autoware::behavior_path_planner::LaneChangePath;
using autoware::behavior_path_planner::lane_change::Debug;
using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
Expand All @@ -40,7 +40,7 @@ MarkerArray createLaneChangingVirtualWallMarker(
const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name,
const rclcpp::Time & now, const std::string & ns);
MarkerArray showFilteredObjects(
const FilteredByLanesExtendedObjects & filtered_objects, const std::string & ns);
const FilteredLanesObjects & filtered_objects, const std::string & ns);
MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area);
MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose);
MarkerArray createDebugMarkerArray(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ using behavior_path_planner::lane_change::CommonDataPtr;
using behavior_path_planner::lane_change::LanesPolygon;
using behavior_path_planner::lane_change::ModuleType;
using behavior_path_planner::lane_change::PathSafetyStatus;
using behavior_path_planner::lane_change::TargetLaneLeadingObjects;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
Expand Down Expand Up @@ -144,8 +145,7 @@ lanelet::BasicPolygon2d create_polygon(
const lanelet::ConstLanelets & lanes, const double start_dist, const double end_dist);

ExtendedPredictedObject transform(
const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters);
const PredictedObject & object, const LaneChangeParameters & lane_change_parameters);

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

bool is_ahead_of_ego(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
const PredictedObject & object);
const ExtendedPredictedObject & object);

bool is_before_terminal(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
const PredictedObject & object);
const ExtendedPredictedObject & object);

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

ExtendedPredictedObjects transform_to_extended_objects(
const CommonDataPtr & common_data_ptr, const std::vector<PredictedObject> & objects);

double get_distance_to_next_regulatory_element(
const CommonDataPtr & common_data_ptr, const bool ignore_crosswalk = false,
const bool ignore_intersection = false);
Expand All @@ -274,7 +271,7 @@ double get_distance_to_next_regulatory_element(
* found, returns the maximum possible double value.
*/
double get_min_dist_to_current_lanes_obj(
const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
const CommonDataPtr & common_data_ptr, const FilteredLanesObjects & filtered_objects,
const double dist_to_target_lane_start, const PathWithLaneId & path);

/**
Expand All @@ -294,8 +291,8 @@ double get_min_dist_to_current_lanes_obj(
* otherwise, false.
*/
bool has_blocking_target_object(
const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects,
const double stop_arc_length, const PathWithLaneId & path);
const TargetLaneLeadingObjects & target_leading_objects, const double stop_arc_length,
const PathWithLaneId & path);

/**
* @brief Checks if the ego vehicle has passed any turn direction within an intersection.
Expand Down Expand Up @@ -342,5 +339,34 @@ std::vector<LineString2d> get_line_string_paths(const ExtendedPredictedObject &
*/
bool has_overtaking_turn_lane_object(
const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects);

/**
* @brief Filters objects based on their positions and velocities relative to the ego vehicle and
* the target lane.
*
* This function evaluates whether an object should be classified as a leading or trailing object
* in the context of a lane change. Objects are filtered based on their lateral distance from
* the ego vehicle, velocity, and whether they are within the target lane or its expanded
* boundaries.
*
* @param common_data_ptr Shared pointer to CommonData containing information about current lanes,
* vehicle dimensions, lane polygons, and behavior parameters.
* @param object An extended predicted object representing a potential obstacle in the environment.
* @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the
* current lanes.
* @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle.
* @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of
* the lane.
* @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or
* outside boundaries).
* @param trailing_objects Reference to a collection for storing trailing objects.
*
* @return true if the object is classified as either leading or trailing, false otherwise.
*/
bool filter_target_lane_objects(
const CommonDataPtr & common_data_ptr, const ExtendedPredictedObject & object,
const double dist_ego_to_current_lanes_center, const bool ahead_of_ego,
const bool before_terminal, TargetLaneLeadingObjects & leading_objects,
ExtendedPredictedObjects & trailing_objects);
} // namespace autoware::behavior_path_planner::utils::lane_change
#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,7 @@
<depend>autoware_rtc_interface</depend>
<depend>autoware_universe_utils</depend>
<depend>pluginlib</depend>
<depend>range-v3</depend>
<depend>rclcpp</depend>
<depend>tier4_planning_msgs</depend>
<depend>visualization_msgs</depend>
Expand Down
Loading

0 comments on commit 25e7d5d

Please sign in to comment.