Skip to content

Commit 54865a9

Browse files
zulfaqar-azmi-t4zulfaqar-azmi-t4
committed
cherry pick lane change PRs v1
* refactor(lane_change): use lane change namespace for structs Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Move lane change namespace to bottom level Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent 8bdeea6 commit 54865a9

File tree

12 files changed

+531
-349
lines changed

12 files changed

+531
-349
lines changed

‎planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
129129
const auto resample_interval = avoidance_parameters_->resample_interval_for_planning;
130130
data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval);
131131

132-
data.current_lanelets = getCurrentLanes();
132+
data.current_lanelets = get_current_lanes();
133133

134134
fillAvoidanceTargetObjects(data, debug);
135135

@@ -268,7 +268,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
268268

269269
double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
270270
{
271-
const auto current_lanes = getCurrentLanes();
271+
const auto current_lanes = get_current_lanes();
272272
if (current_lanes.empty()) {
273273
RCLCPP_DEBUG(logger_, "no empty lanes");
274274
return std::numeric_limits<double>::infinity();

‎planning/behavior_path_lane_change_module/README.md

+27-57
Original file line numberDiff line numberDiff line change
@@ -331,8 +331,7 @@ title NormalLaneChange::filterObjects Method Execution Flow
331331
start
332332
333333
group "Filter Objects by Class" {
334-
:Iterate through each object in objects list;
335-
while (has not finished iterating through object list) is (TRUE)
334+
while (has not finished iterating through predicted object list) is (TRUE)
336335
if (current object type != param.object_types_to_check?) then (TRUE)
337336
#LightPink:Remove current object;
338337
else (FALSE)
@@ -341,17 +340,15 @@ endif
341340
end while
342341
end group
343342
344-
if (object list is empty?) then (TRUE)
343+
if (predicted object list is empty?) then (TRUE)
345344
:Return empty result;
346345
stop
347346
else (FALSE)
348347
endif
349348
350349
group "Filter Oncoming Objects" #PowderBlue {
351-
:Iterate through each object in target lane objects list;
352-
while (has not finished iterating through object list?) is (TRUE)
353-
:check object's yaw with reference to ego's yaw.;
354-
if (yaw difference < 90 degree?) then (TRUE)
350+
while (has not finished iterating through predicted object list?) is (TRUE)
351+
if (object's yaw with reference to ego's yaw difference < 90 degree?) then (TRUE)
355352
:Keep current object;
356353
else (FALSE)
357354
if (object is stopping?) then (TRUE)
@@ -363,53 +360,35 @@ endif
363360
endwhile
364361
end group
365362
366-
if (object list is empty?) then (TRUE)
367-
:Return empty result;
368-
stop
369-
else (FALSE)
370-
endif
371-
372-
group "Filter Objects Ahead Terminal" #Beige {
373-
:Calculate lateral distance from ego to current lanes center;
374-
375-
:Iterate through each object in objects list;
376-
while (has not finished iterating through object list) is (TRUE)
377-
:Get current object's polygon;
378-
:Initialize distance to terminal from object to max;
379-
while (has not finished iterating through object polygon's vertices) is (TRUE)
380-
:Calculate object's lateral distance to end of lane;
381-
:Update minimum distance to terminal from object;
382-
end while
383-
if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE)
384-
#LightPink:Remove current object;
385-
else (FALSE)
386-
endif
387-
end while
388-
end group
389-
390-
if (object list is empty?) then (TRUE)
363+
if (predicted object list is empty?) then (TRUE)
391364
:Return empty result;
392365
stop
393366
else (FALSE)
394367
endif
395368
396369
group "Filter Objects By Lanelets" #LightGreen {
397370
398-
:Iterate through each object in objects list;
399-
while (has not finished iterating through object list) is (TRUE)
400-
:lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.;
401-
if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE)
402-
:Add to target_lane_objects;
403-
else (FALSE)
404-
if (Object overlaps with backward target lanes?) then (TRUE)
371+
while (has not finished iterating through predicted object list) is (TRUE)
372+
:Calculate lateral distance diff;
373+
if (Object in target lane polygon?) then (TRUE)
374+
if (lateral distance diff > half of ego's width?) then (TRUE)
375+
if (Object's physical position is before terminal point?) then (TRUE)
405376
:Add to target_lane_objects;
406377
else (FALSE)
407-
if (Object in current lane polygon?) then (TRUE)
408-
:Add to current_lane_objects;
409-
else (FALSE)
410-
:Add to other_lane_objects;
411-
endif
412378
endif
379+
else (FALSE)
380+
endif
381+
else (FALSE)
382+
endif
383+
384+
if (Object overlaps with backward target lanes?) then (TRUE)
385+
:Add to target_lane_objects;
386+
else (FALSE)
387+
if (Object in current lane polygon?) then (TRUE)
388+
:Add to current_lane_objects;
389+
else (FALSE)
390+
:Add to other_lane_objects;
391+
endif
413392
endif
414393
end while
415394
@@ -426,13 +405,10 @@ endif
426405
427406
group "Filter Target Lanes' objects" #LightCyan {
428407
429-
:Iterate through each object in target lane objects list;
430-
while (has not finished iterating through object list) is (TRUE)
431-
:check object's velocity;
408+
while (has not finished iterating through target lanes' object list) is (TRUE)
432409
if(velocity is within threshold?) then (TRUE)
433410
:Keep current object;
434411
else (FALSE)
435-
:check whether object is ahead of ego;
436412
if(object is ahead of ego?) then (TRUE)
437413
:keep current object;
438414
else (FALSE)
@@ -444,11 +420,8 @@ end group
444420
445421
group "Filter Current Lanes' objects" #LightYellow {
446422
447-
:Iterate through each object in current lane objects list;
448-
while (has not finished iterating through object list) is (TRUE)
449-
:check object's velocity;
423+
while (has not finished iterating through current lanes' object list) is (TRUE)
450424
if(velocity is within threshold?) then (TRUE)
451-
:check whether object is ahead of ego;
452425
if(object is ahead of ego?) then (TRUE)
453426
:keep current object;
454427
else (FALSE)
@@ -462,11 +435,8 @@ end group
462435
463436
group "Filter Other Lanes' objects" #Lavender {
464437
465-
:Iterate through each object in other lane objects list;
466-
while (has not finished iterating through object list) is (TRUE)
467-
:check object's velocity;
438+
while (has not finished iterating through other lanes' object list) is (TRUE)
468439
if(velocity is within threshold?) then (TRUE)
469-
:check whether object is ahead of ego;
470440
if(object is ahead of ego?) then (TRUE)
471441
:keep current object;
472442
else (FALSE)
@@ -478,7 +448,7 @@ while (has not finished iterating through object list) is (TRUE)
478448
endwhile
479449
end group
480450
481-
:Trasform the objects into extended predicted object and return them as lane_change_target_objects;
451+
:Transform the objects into extended predicted object and return them as lane_change_target_objects;
482452
stop
483453
484454
@enduml

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

+10-11
Original file line numberDiff line numberDiff line change
@@ -30,10 +30,10 @@ using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject
3030
using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped;
3131
using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
3232
using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
33-
using data::lane_change::LanesPolygon;
3433
using geometry_msgs::msg::Point;
3534
using geometry_msgs::msg::Pose;
3635
using geometry_msgs::msg::Twist;
36+
using lane_change::LanesPolygon;
3737
using route_handler::Direction;
3838
using utils::path_safety_checker::ExtendedPredictedObjects;
3939

@@ -50,6 +50,8 @@ class NormalLaneChange : public LaneChangeBase
5050
NormalLaneChange & operator=(NormalLaneChange &&) = delete;
5151
~NormalLaneChange() override = default;
5252

53+
void update_lanes(const bool is_approved) final;
54+
5355
void updateLaneChangeStatus() override;
5456

5557
std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const override;
@@ -104,8 +106,6 @@ class NormalLaneChange : public LaneChangeBase
104106
TurnSignalInfo get_current_turn_signal_info() override;
105107

106108
protected:
107-
lanelet::ConstLanelets getCurrentLanes() const override;
108-
109109
lanelet::ConstLanelets getLaneChangeLanes(
110110
const lanelet::ConstLanelets & current_lanes, Direction direction) const override;
111111

@@ -123,18 +123,12 @@ class NormalLaneChange : public LaneChangeBase
123123
const LaneChangeLanesFilteredObjects & predicted_objects,
124124
const lanelet::ConstLanelets & current_lanes) const;
125125

126-
LaneChangeLanesFilteredObjects filterObjects(
127-
const lanelet::ConstLanelets & current_lanes,
128-
const lanelet::ConstLanelets & target_lanes) const;
126+
LaneChangeLanesFilteredObjects filterObjects() const;
129127

130128
void filterOncomingObjects(PredictedObjects & objects) const;
131129

132-
void filterAheadTerminalObjects(
133-
PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const;
134-
135130
void filterObjectsByLanelets(
136-
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
137-
const lanelet::ConstLanelets & target_lanes,
131+
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path,
138132
std::vector<PredictedObject> & current_lane_objects,
139133
std::vector<PredictedObject> & target_lane_objects,
140134
std::vector<PredictedObject> & other_lane_objects) const;
@@ -202,6 +196,11 @@ class NormalLaneChange : public LaneChangeBase
202196

203197
double getStopTime() const { return stop_time_; }
204198

199+
const lanelet::ConstLanelets & get_target_lanes() const
200+
{
201+
return common_data_ptr_->lanes_ptr->target;
202+
}
203+
205204
double stop_time_{0.0};
206205
};
207206
} // namespace behavior_path_planner

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

+37-7
Original file line numberDiff line numberDiff line change
@@ -37,10 +37,10 @@
3737
namespace behavior_path_planner
3838
{
3939
using autoware_auto_planning_msgs::msg::PathWithLaneId;
40-
using data::lane_change::PathSafetyStatus;
4140
using geometry_msgs::msg::Point;
4241
using geometry_msgs::msg::Pose;
4342
using geometry_msgs::msg::Twist;
43+
using lane_change::PathSafetyStatus;
4444
using route_handler::Direction;
4545
using tier4_autoware_utils::StopWatch;
4646

@@ -50,7 +50,10 @@ class LaneChangeBase
5050
LaneChangeBase(
5151
std::shared_ptr<LaneChangeParameters> parameters, LaneChangeModuleType type,
5252
Direction direction)
53-
: lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type}
53+
: lane_change_parameters_{std::move(parameters)},
54+
common_data_ptr_{std::make_shared<lane_change::CommonData>()},
55+
direction_{direction},
56+
type_{type}
5457
{
5558
}
5659

@@ -60,6 +63,8 @@ class LaneChangeBase
6063
LaneChangeBase & operator=(LaneChangeBase &&) = delete;
6164
virtual ~LaneChangeBase() = default;
6265

66+
virtual void update_lanes(const bool is_approved) = 0;
67+
6368
virtual void updateLaneChangeStatus() = 0;
6469

6570
virtual std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const = 0;
@@ -126,14 +131,19 @@ class LaneChangeBase
126131

127132
const LaneChangeStatus & getLaneChangeStatus() const { return status_; }
128133

129-
const data::lane_change::Debug & getDebugData() const { return lane_change_debug_; }
134+
const lane_change::Debug & getDebugData() const { return lane_change_debug_; }
130135

131136
const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
132137

133138
const Point & getEgoPosition() const { return getEgoPose().position; }
134139

135140
const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; }
136141

142+
const lanelet::ConstLanelets & get_current_lanes() const
143+
{
144+
return common_data_ptr_->lanes_ptr->current;
145+
}
146+
137147
const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; }
138148

139149
LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; }
@@ -151,7 +161,28 @@ class LaneChangeBase
151161

152162
bool isValidPath() const { return status_.is_valid_path; }
153163

154-
void setData(const std::shared_ptr<const PlannerData> & data) { planner_data_ = data; }
164+
void setData(const std::shared_ptr<const PlannerData> & data)
165+
{
166+
planner_data_ = data;
167+
if (!common_data_ptr_->bpp_param_ptr) {
168+
common_data_ptr_->bpp_param_ptr =
169+
std::make_shared<BehaviorPathPlannerParameters>(data->parameters);
170+
}
171+
172+
if (!common_data_ptr_->lanes_ptr) {
173+
common_data_ptr_->lanes_ptr = std::make_shared<lane_change::Lanes>();
174+
}
175+
176+
if (!common_data_ptr_->lanes_polygon_ptr) {
177+
common_data_ptr_->lanes_polygon_ptr = std::make_shared<lane_change::LanesPolygon>();
178+
}
179+
180+
common_data_ptr_->self_odometry_ptr = data->self_odometry;
181+
common_data_ptr_->route_handler_ptr = data->route_handler;
182+
common_data_ptr_->lc_param_ptr = lane_change_parameters_;
183+
common_data_ptr_->lc_type = type_;
184+
common_data_ptr_->direction = direction_;
185+
}
155186

156187
void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; }
157188

@@ -190,8 +221,6 @@ class LaneChangeBase
190221
virtual TurnSignalInfo get_current_turn_signal_info() = 0;
191222

192223
protected:
193-
virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
194-
195224
virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
196225

197226
virtual PathWithLaneId getPrepareSegment(
@@ -219,6 +248,7 @@ class LaneChangeBase
219248
std::shared_ptr<LaneChangeParameters> lane_change_parameters_{};
220249
std::shared_ptr<LaneChangePath> abort_path_{};
221250
std::shared_ptr<const PlannerData> planner_data_{};
251+
lane_change::CommonDataPtr common_data_ptr_{};
222252
BehaviorModuleOutput prev_module_output_{};
223253
std::optional<Pose> lane_change_stop_pose_{std::nullopt};
224254

@@ -233,7 +263,7 @@ class LaneChangeBase
233263
LaneChangeModuleType type_{LaneChangeModuleType::NORMAL};
234264

235265
mutable StopWatch<std::chrono::milliseconds> stop_watch_;
236-
mutable data::lane_change::Debug lane_change_debug_;
266+
mutable lane_change::Debug lane_change_debug_;
237267

238268
rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr());
239269
mutable rclcpp::Clock clock_{RCL_ROS_TIME};

0 commit comments

Comments
 (0)