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): add new structs (TransientData) to store commonly used values #8849

Closed
wants to merge 18 commits into from
Closed

refactor(lane_change): add new structs (TransientData) to store commonly used values #8849

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
b461626
renaming min_lane_change_buffer to current_lc_buffer
zulfaqar-azmi-t4 Sep 10, 2024
030a1ab
change argument for the general function
zulfaqar-azmi-t4 Sep 10, 2024
1cdfb95
rename function name
zulfaqar-azmi-t4 Sep 10, 2024
f910d0a
do it also for maximum
zulfaqar-azmi-t4 Sep 10, 2024
0781964
change parameter type for lc param
zulfaqar-azmi-t4 Sep 10, 2024
564edde
add transient data
zulfaqar-azmi-t4 Sep 10, 2024
4cf9c92
ego dist to terminal end
zulfaqar-azmi-t4 Sep 11, 2024
765d744
style(pre-commit): autofix
pre-commit-ci[bot] Sep 11, 2024
3b6a295
refactor maximum buffer
zulfaqar-azmi-t4 Sep 12, 2024
00c9d47
ego to end of lane
zulfaqar-azmi-t4 Sep 12, 2024
26dd220
fix terminal path not generated
zulfaqar-azmi-t4 Sep 13, 2024
877459c
return boundary, common data ptr in isLCrequired and ablc fix
zulfaqar-azmi-t4 Sep 19, 2024
22e32bd
clang-tidy
zulfaqar-azmi-t4 Sep 19, 2024
d5b7eba
change to dist to terminal start
zulfaqar-azmi-t4 Sep 19, 2024
e2378a9
fix header file
zulfaqar-azmi-t4 Sep 19, 2024
2a1b2f4
fix some bug
zulfaqar-azmi-t4 Sep 19, 2024
78de706
fix abort computation
zulfaqar-azmi-t4 Sep 20, 2024
bb4f783
fix some computation issue
zulfaqar-azmi-t4 Sep 20, 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
16 changes: 6 additions & 10 deletions ...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const

const auto & nearest_object = data.target_objects.front();
const auto minimum_avoid_length = calcMinAvoidanceLength(nearest_object);
const auto minimum_lane_change_length = calcMinimumLaneChangeLength();
const auto minimum_lane_change_length = calc_minimum_dist_buffer();

lane_change_debug_.execution_area = createExecutionArea(
getCommonParam().vehicle_info, getEgoPose(),
Expand Down Expand Up @@ -273,16 +273,12 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
return avoidance_helper_->getMinAvoidanceDistance(shift_length);
}

double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
double AvoidanceByLaneChange::calc_minimum_dist_buffer() const
{
const auto current_lanes = get_current_lanes();
if (current_lanes.empty()) {
RCLCPP_DEBUG(logger_, "no empty lanes");
return std::numeric_limits<double>::infinity();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If I understand the code correctly, the new return value in case current_lanes is empty is 0 compared to infinity() previously. Is this okay ?

  • the calc_lc_length_and_dist_buffer() returns {} when lanes is empty.
  • {} uses the default constructor of pair which uses the default constructor of Boundary which sets min = 0.0.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@zulfaqar-azmi-t4 did you check this issue ?

}

return utils::lane_change::calculation::calc_minimum_lane_change_length(
getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_);
const auto lc_length_and_dist_buffer =
utils::lane_change::calculation::calc_lc_length_and_dist_buffer(
common_data_ptr_, get_current_lanes());
return std::get<1>(lc_length_and_dist_buffer).min;
Comment on lines +278 to +281
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

(minor) I think you can do something like this which is a bit easier to read.

Suggested change
const auto lc_length_and_dist_buffer =
utils::lane_change::calculation::calc_lc_length_and_dist_buffer(
common_data_ptr_, get_current_lanes());
return std::get<1>(lc_length_and_dist_buffer).min;
const auto [_, dist_buffer] =
utils::lane_change::calculation::calc_lc_length_and_dist_buffer(
common_data_ptr_, get_current_lanes());
return dist_buffer.min;

}

double AvoidanceByLaneChange::calcLateralOffset() const
Expand Down
2 changes: 1 addition & 1 deletion ...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ class AvoidanceByLaneChange : public NormalLaneChange
void fillAvoidanceTargetObjects(AvoidancePlanningData & data, AvoidanceDebugData & debug) const;

double calcMinAvoidanceLength(const ObjectData & nearest_object) const;
double calcMinimumLaneChangeLength() const;
double calc_minimum_dist_buffer() const;
double calcLateralOffset() const;
};
} // namespace autoware::behavior_path_planner
Expand Down
14 changes: 3 additions & 11 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 @@ -54,6 +54,8 @@ class NormalLaneChange : public LaneChangeBase

void update_lanes(const bool is_approved) final;

void update_transient_data() final;

void update_filtered_objects() final;

void updateLaneChangeStatus() override;
Expand Down Expand Up @@ -87,10 +89,6 @@ class NormalLaneChange : public LaneChangeBase

bool isRequiredStop(const bool is_trailing_object) override;

bool isNearEndOfCurrentLanes(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
const double threshold) const override;

bool hasFinishedLaneChange() const override;

bool isAbleToReturnCurrentLane() const override;
Expand Down Expand Up @@ -125,9 +123,7 @@ class NormalLaneChange : public LaneChangeBase
const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes) const;

std::vector<double> calcPrepareDuration(
const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes) const;
std::vector<double> calcPrepareDuration() const;

lane_change::TargetObjects getTargetObjects(
const FilteredByLanesExtendedObjects & predicted_objects,
Expand All @@ -149,10 +145,6 @@ class NormalLaneChange : public LaneChangeBase
const double target_lane_length, const double lane_changing_length,
const double lane_changing_velocity, const double buffer_for_next_lane_change) const;

bool hasEnoughLength(
const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const;

bool hasEnoughLengthToCrosswalk(
const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;

Expand Down
6 changes: 2 additions & 4 deletions ...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 @@ -67,6 +67,8 @@ class LaneChangeBase

virtual void update_lanes(const bool is_approved) = 0;

virtual void update_transient_data() = 0;

virtual void update_filtered_objects() = 0;

virtual void updateLaneChangeStatus() = 0;
Expand Down Expand Up @@ -110,10 +112,6 @@ class LaneChangeBase
virtual PathSafetyStatus evaluateApprovedPathWithUnsafeHysteresis(
PathSafetyStatus approve_path_safety_status) = 0;

virtual bool isNearEndOfCurrentLanes(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
const double threshold) const = 0;

virtual bool isStoppedAtRedTrafficLight() const = 0;

virtual bool calcAbortPath() = 0;
Expand Down
34 changes: 3 additions & 31 deletions ...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,26 +22,10 @@ namespace autoware::behavior_path_planner::utils::lane_change::calculation
{
using autoware::route_handler::Direction;
using autoware::route_handler::RouteHandler;
using behavior_path_planner::lane_change::Boundary;
using behavior_path_planner::lane_change::CommonDataPtr;
using behavior_path_planner::lane_change::LCParamPtr;

/**
* @brief Calculates the distance from the ego vehicle to the terminal point.
*
* This function computes the distance from the current position of the ego vehicle
* to either the goal pose or the end of the current lane, depending on whether
* the vehicle's current lane is within the goal section.
*
* @param common_data_ptr Shared pointer to a CommonData structure, which should include:
* - Non null `lanes_ptr` that points to the current lanes data.
* - Non null `self_odometry_ptr` that contains the current pose of the ego vehicle.
* - Non null `route_handler_ptr` that contains the goal pose of the route.
*
* @return The distance to the terminal point (either the goal pose or the end of the current lane)
* in meters.
*/
double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr);

double calc_dist_from_pose_to_terminal_end(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes,
const Pose & src_pose);
Expand Down Expand Up @@ -118,20 +102,8 @@ double calc_ego_dist_to_lanes_start(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes);

double calc_minimum_lane_change_length(
const LaneChangeParameters & lane_change_parameters, const std::vector<double> & shift_intervals);

double calc_minimum_lane_change_length(
const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lane,
const LaneChangeParameters & lane_change_parameters, Direction direction);

double calc_maximum_lane_change_length(
const double current_velocity, const LaneChangeParameters & lane_change_parameters,
const std::vector<double> & shift_intervals, const double max_acc);

double calc_maximum_lane_change_length(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelet & current_terminal_lanelet,
const double max_acc);
std::pair<Boundary, Boundary> calc_lc_length_and_dist_buffer(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes);
} // namespace autoware::behavior_path_planner::utils::lane_change::calculation

#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_
35 changes: 35 additions & 0 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 @@ -27,6 +27,7 @@
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_core/primitives/Polygon.h>

#include <limits>
#include <memory>
#include <utility>
#include <vector>
Expand Down Expand Up @@ -196,6 +197,7 @@ struct PhaseInfo
struct Lanes
{
bool current_lane_in_goal_section{false};
bool target_lane_in_goal_section{false};
lanelet::ConstLanelets current;
lanelet::ConstLanelets target_neighbor;
lanelet::ConstLanelets target;
Expand Down Expand Up @@ -274,6 +276,25 @@ struct LanesPolygon
std::vector<lanelet::BasicPolygon2d> preceding_target;
};

struct Boundary
{
double min{0.0};
double max{0.0};
};

struct TransientData
{
Boundary acc;
Boundary lane_changing_length;
Boundary current_dist_buffer;
Boundary next_lc_buffer;
double dist_from_ego_to_current_terminal_end{std::numeric_limits<double>::min()};
double dist_from_ego_to_current_terminal_start{std::numeric_limits<double>::min()};
double maximum_prepare_length{std::numeric_limits<double>::max()};

bool is_ego_near_current_terminal_start{false};
};

using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
using LCParamPtr = std::shared_ptr<Parameters>;
Expand All @@ -288,6 +309,8 @@ struct CommonData
LCParamPtr lc_param_ptr;
LanesPtr lanes_ptr;
LanesPolygonPtr lanes_polygon_ptr;
TransientData transient_data;
PathWithLaneId current_lanes_path;
ModuleType lc_type;
Direction direction;

Expand All @@ -305,6 +328,18 @@ struct CommonData
const auto y = get_ego_twist().linear.y;
return std::hypot(x, y);
}

[[nodiscard]] bool is_data_available() const
{
return route_handler_ptr && self_odometry_ptr && bpp_param_ptr && lc_param_ptr && lanes_ptr &&
lanes_polygon_ptr;
}

[[nodiscard]] bool is_lanes_available() const
{
return lanes_ptr && !lanes_ptr->current.empty() && !lanes_ptr->target.empty() &&
!lanes_ptr->target_neighbor.empty();
}
};
using CommonDataPtr = std::shared_ptr<CommonData>;
} // namespace autoware::behavior_path_planner::lane_change
Expand Down
6 changes: 1 addition & 5 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 @@ -52,6 +52,7 @@ using autoware_perception_msgs::msg::PredictedPath;
using behavior_path_planner::lane_change::CommonDataPtr;
using behavior_path_planner::lane_change::LanesPolygon;
using behavior_path_planner::lane_change::PathSafetyStatus;
using behavior_path_planner::lane_change::TransientData;
using geometry_msgs::msg::Point;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
Expand Down Expand Up @@ -130,11 +131,6 @@ std::vector<DrivableLanes> generateDrivableLanes(

double getLateralShift(const LaneChangePath & path);

bool hasEnoughLengthToLaneChangeAfterAbort(
const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelets & current_lanes,
const Pose & curent_pose, const double abort_return_dist,
const LaneChangeParameters & lane_change_parameters, const Direction direction);

CandidateOutput assignToCandidate(
const LaneChangePath & lane_change_path, const Point & ego_position);
std::optional<lanelet::ConstLanelet> getLaneChangeTargetLane(
Expand Down
1 change: 1 addition & 0 deletions 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 @@ -76,6 +76,7 @@ void LaneChangeInterface::updateData()
universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING);
module_type_->update_transient_data();
module_type_->update_filtered_objects();
module_type_->updateSpecialData();

Expand Down
Loading
Loading