Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

refactor(lane_change): separate leading and trailing objects #8214

Merged
merged 2 commits into from
Aug 13, 2024
Merged

refactor(lane_change): separate leading and trailing objects #8214

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
d30236b
refactor(lane_change): separate leading and trailing objects
zulfaqar-azmi-t4 Jul 23, 2024
5118861
Refactor to use common function
zulfaqar-azmi-t4 Aug 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 7 additions & 10 deletions ...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ class NormalLaneChange : public LaneChangeBase
PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis(
PathSafetyStatus approved_path_safety_status) override;

bool isRequiredStop(const bool is_object_coming_from_rear) override;
bool isRequiredStop(const bool is_trailing_object) override;

bool isNearEndOfCurrentLanes(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
Expand Down Expand Up @@ -120,19 +120,16 @@ class NormalLaneChange : public LaneChangeBase
const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes) const;

ExtendedPredictedObjects getTargetObjects(
const LaneChangeLanesFilteredObjects & predicted_objects,
lane_change::TargetObjects getTargetObjects(
const FilteredByLanesExtendedObjects & predicted_objects,
const lanelet::ConstLanelets & current_lanes) const;

LaneChangeLanesFilteredObjects filterObjects() const;
FilteredByLanesExtendedObjects filterObjects() const;

void filterOncomingObjects(PredictedObjects & objects) const;

void filterObjectsByLanelets(
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path,
std::vector<PredictedObject> & current_lane_objects,
std::vector<PredictedObject> & target_lane_objects,
std::vector<PredictedObject> & other_lane_objects) const;
FilteredByLanesObjects filterObjectsByLanelets(
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path) const;

PathWithLaneId getPrepareSegment(
const lanelet::ConstLanelets & current_lanes, const double backward_path_length,
Expand Down Expand Up @@ -170,7 +167,7 @@ class NormalLaneChange : public LaneChangeBase

PathSafetyStatus isLaneChangePathSafe(
const LaneChangePath & lane_change_path,
const ExtendedPredictedObjects & collision_check_objects,
const lane_change::TargetObjects & collision_check_objects,
const utils::path_safety_checker::RSSparams & rss_params,
CollisionCheckDebugMap & debug_data) const;

Expand Down
2 changes: 1 addition & 1 deletion ...lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ class LaneChangeBase

virtual bool isEgoOnPreparePhase() const = 0;

virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0;
virtual bool isRequiredStop(const bool is_trailing_object) = 0;

virtual PathSafetyStatus isApprovedPathSafe() const = 0;

Expand Down
41 changes: 36 additions & 5 deletions ...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -212,11 +212,33 @@ struct Info
double terminal_lane_changing_velocity{0.0};
};

template <typename Object>
struct LanesObjects
{
ExtendedPredictedObjects current_lane{};
ExtendedPredictedObjects target_lane{};
ExtendedPredictedObjects other_lane{};
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))
{
}
};

struct TargetObjects
{
ExtendedPredictedObjects leading;
ExtendedPredictedObjects trailing;
TargetObjects(ExtendedPredictedObjects leading, ExtendedPredictedObjects trailing)
: leading(std::move(leading)), trailing(std::move(trailing))
{
}
};

enum class ModuleType {
Expand All @@ -228,7 +250,13 @@ enum class ModuleType {
struct PathSafetyStatus
{
bool is_safe{true};
bool is_object_coming_from_rear{false};
bool is_trailing_object{false};

PathSafetyStatus() = default;
PathSafetyStatus(const bool is_safe, const bool is_trailing_object)
: is_safe(is_safe), is_trailing_object(is_trailing_object)
{
}
};

struct LanesPolygon
Expand Down Expand Up @@ -277,12 +305,15 @@ using CommonDataPtr = std::shared_ptr<CommonData>;

namespace autoware::behavior_path_planner
{
using autoware_perception_msgs::msg::PredictedObject;
using utils::path_safety_checker::ExtendedPredictedObjects;
using LaneChangeModuleType = lane_change::ModuleType;
using LaneChangeParameters = lane_change::Parameters;
using LaneChangeStates = lane_change::States;
using LaneChangePhaseInfo = lane_change::PhaseInfo;
using LaneChangeInfo = lane_change::Info;
using LaneChangeLanesFilteredObjects = lane_change::LanesObjects;
using FilteredByLanesObjects = lane_change::LanesObjects<std::vector<PredictedObject>>;
using FilteredByLanesExtendedObjects = lane_change::LanesObjects<ExtendedPredictedObjects>;
using LateralAccelerationMap = lane_change::LateralAccelerationMap;
} // namespace autoware::behavior_path_planner

Expand Down
5 changes: 3 additions & 2 deletions ...e_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp
View file
Open in desktop
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;
LaneChangeLanesFilteredObjects filtered_objects;
FilteredByLanesExtendedObjects filtered_objects;
geometry_msgs::msg::Polygon execution_area;
geometry_msgs::msg::Pose ego_pose;
lanelet::ConstLanelets current_lanes;
Expand All @@ -55,7 +55,8 @@ struct Debug
collision_check_objects.clear();
collision_check_objects_after_approval.clear();
filtered_objects.current_lane.clear();
filtered_objects.target_lane.clear();
filtered_objects.target_lane_leading.clear();
filtered_objects.target_lane_trailing.clear();
filtered_objects.other_lane.clear();
execution_area.points.clear();
current_lanes.clear();
Expand Down
5 changes: 2 additions & 3 deletions ...th_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

namespace marker_utils::lane_change_markers
{
using autoware::behavior_path_planner::FilteredByLanesExtendedObjects;
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 @@ -39,9 +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 ExtendedPredictedObjects & current_lane_objects,
const ExtendedPredictedObjects & target_lane_objects,
const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
const FilteredByLanesExtendedObjects & 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
4 changes: 4 additions & 0 deletions ...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -312,6 +312,10 @@ bool is_before_terminal(
const PredictedObject & 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,
const bool check_prepare_phase);
} // namespace autoware::behavior_path_planner::utils::lane_change

namespace autoware::behavior_path_planner::utils::lane_change::debug
Expand Down
2 changes: 1 addition & 1 deletion planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -307,7 +307,7 @@ bool LaneChangeInterface::canTransitFailureState()
return false;
}

if (module_type_->isRequiredStop(post_process_safety_status_.is_object_coming_from_rear)) {
if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) {
log_debug_throttled("Module require stopping");
}

Expand Down
Loading
Loading