Skip to content

Commit 8c10227

Browse files
committedMar 20, 2024
Add new parameter, also create function
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent f24c07c commit 8c10227

File tree

4 files changed

+37
-30
lines changed

4 files changed

+37
-30
lines changed
 

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -174,6 +174,8 @@ class NormalLaneChange : public LaneChangeBase
174174

175175
bool isVehicleStuck(const lanelet::ConstLanelets & current_lanes) const;
176176

177+
bool check_prepare_phase() const;
178+
177179
double calcMaximumLaneChangeLength(
178180
const lanelet::ConstLanelet & current_terminal_lanelet, const double max_acc) const;
179181

‎planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -112,7 +112,8 @@ struct LaneChangeParameters
112112
double max_longitudinal_acc{1.0};
113113

114114
// collision check
115-
bool enable_prepare_segment_collision_check{true};
115+
bool enable_prepare_segment_collision_check_in_general_areas{false};
116+
bool enable_prepare_segment_collision_check_in_intersection{true};
116117
double prepare_segment_ignore_object_velocity_thresh{0.1};
117118
bool check_objects_on_current_lanes{true};
118119
bool check_objects_on_other_lanes{true};

‎planning/behavior_path_lane_change_module/src/manager.cpp

+4-2
Original file line numberDiff line numberDiff line change
@@ -68,8 +68,10 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
6868
p.max_longitudinal_acc = getOrDeclareParameter<double>(*node, parameter("max_longitudinal_acc"));
6969

7070
// collision check
71-
p.enable_prepare_segment_collision_check =
72-
getOrDeclareParameter<bool>(*node, parameter("enable_prepare_segment_collision_check"));
71+
p.enable_prepare_segment_collision_check_in_general_areas = getOrDeclareParameter<bool>(
72+
*node, parameter("enable_prepare_segment_collision_check.general_areas"));
73+
p.enable_prepare_segment_collision_check_in_intersection = getOrDeclareParameter<bool>(
74+
*node, parameter("enable_prepare_segment_collision_check.intersection"));
7375
p.prepare_segment_ignore_object_velocity_thresh = getOrDeclareParameter<double>(
7476
*node, parameter("prepare_segment_ignore_object_velocity_thresh"));
7577
p.check_objects_on_current_lanes =

‎planning/behavior_path_lane_change_module/src/scene.cpp

+29-27
Original file line numberDiff line numberDiff line change
@@ -862,38 +862,26 @@ LaneChangeTargetObjects NormalLaneChange::getTargetObjects(
862862
target_objects.other_lane.reserve(target_obj_index.other_lane.size());
863863

864864
// objects in current lane
865-
866-
lanelet::ConstLanelet current_lane;
867-
if (getRouteHandler()->getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
868-
}
869-
const auto ego_footprint =
870-
utils::lane_change::getEgoCurrentFootprint(getEgoPose(), common_parameters.vehicle_info);
871-
const auto is_ego_within_intersection =
872-
utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
873-
874-
const auto check_prepare_phase =
875-
is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check;
876-
877865
for (const auto & obj_idx : target_obj_index.current_lane) {
878866
const auto extended_object = utils::lane_change::transform(
879867
objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
880-
check_prepare_phase);
868+
check_prepare_phase());
881869
target_objects.current_lane.push_back(extended_object);
882870
}
883871

884872
// objects in target lane
885873
for (const auto & obj_idx : target_obj_index.target_lane) {
886874
const auto extended_object = utils::lane_change::transform(
887875
objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
888-
check_prepare_phase);
876+
check_prepare_phase());
889877
target_objects.target_lane.push_back(extended_object);
890878
}
891879

892880
// objects in other lane
893881
for (const auto & obj_idx : target_obj_index.other_lane) {
894882
const auto extended_object = utils::lane_change::transform(
895883
objects.objects.at(obj_idx), common_parameters, *lane_change_parameters_,
896-
check_prepare_phase);
884+
check_prepare_phase());
897885
target_objects.other_lane.push_back(extended_object);
898886
}
899887

@@ -951,25 +939,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
951939
target_backward_polygons.push_back(lane_polygon);
952940
}
953941

954-
lanelet::ConstLanelet current_lane;
955-
if (!route_handler.getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
956-
}
957-
const auto ego_footprint =
958-
utils::lane_change::getEgoCurrentFootprint(getEgoPose(), common_parameters.vehicle_info);
959-
const auto is_ego_within_intersection =
960-
utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
961-
962-
const auto check_prepare_phase =
963-
is_ego_within_intersection || lane_change_parameters_->enable_prepare_segment_collision_check;
964-
965942
LaneChangeTargetObjectIndices filtered_obj_indices;
966943
for (size_t i = 0; i < objects.objects.size(); ++i) {
967944
const auto & object = objects.objects.at(i);
968945
const auto obj_velocity_norm = std::hypot(
969946
object.kinematics.initial_twist_with_covariance.twist.linear.x,
970947
object.kinematics.initial_twist_with_covariance.twist.linear.y);
971948
const auto extended_object = utils::lane_change::transform(
972-
object, common_parameters, *lane_change_parameters_, check_prepare_phase);
949+
object, common_parameters, *lane_change_parameters_, check_prepare_phase());
973950

974951
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object);
975952

@@ -2108,4 +2085,29 @@ void NormalLaneChange::updateStopTime()
21082085
stop_watch_.tic("stop_time");
21092086
}
21102087

2088+
bool NormalLaneChange::check_prepare_phase() const
2089+
{
2090+
const auto & route_handler = getRouteHandler();
2091+
const auto & vehicle_info = getCommonParam().vehicle_info;
2092+
2093+
const auto check_prepare_phase_in_intersection = std::invoke([&]() {
2094+
if (!lane_change_parameters_->enable_prepare_segment_collision_check_in_intersection) {
2095+
return false;
2096+
}
2097+
2098+
lanelet::ConstLanelet current_lane;
2099+
if (!route_handler->getClosestLaneletWithinRoute(getEgoPose(), &current_lane)) {
2100+
RCLCPP_DEBUG(logger_, "unable to retreive current lane");
2101+
return false;
2102+
}
2103+
2104+
const auto ego_footprint =
2105+
utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info);
2106+
return utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint);
2107+
});
2108+
2109+
return check_prepare_phase_in_intersection ||
2110+
lane_change_parameters_->enable_prepare_segment_collision_check_in_general_areas;
2111+
}
2112+
21112113
} // namespace behavior_path_planner

0 commit comments

Comments
 (0)