Skip to content

Commit d571fc4

Browse files
zulfaqar-azmi-t4zulfaqar-azmi-t4
authored
refactor(lane_change): update lanes and its polygons only when it's updated (autowarefoundation#7989)
* refactor(lane_change): compute lanes and polygon only when updated Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Revert accidental changesd This reverts commit cbfd9ae. Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * fix spell check Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Make a common getter for current lanes Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * add target lanes getter Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * some minor function refactoring Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> --------- Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent 771f71d commit d571fc4

File tree

9 files changed

+246
-172
lines changed

9 files changed

+246
-172
lines changed

‎planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData(
134134
const auto resample_interval = avoidance_parameters_->resample_interval_for_planning;
135135
data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval);
136136

137-
data.current_lanelets = getCurrentLanes();
137+
data.current_lanelets = get_current_lanes();
138138

139139
fillAvoidanceTargetObjects(data, debug);
140140

@@ -274,7 +274,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_
274274

275275
double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const
276276
{
277-
const auto current_lanes = getCurrentLanes();
277+
const auto current_lanes = get_current_lanes();
278278
if (current_lanes.empty()) {
279279
RCLCPP_DEBUG(logger_, "no empty lanes");
280280
return std::numeric_limits<double>::infinity();

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp

+8-5
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,8 @@ class NormalLaneChange : public LaneChangeBase
5151
NormalLaneChange & operator=(NormalLaneChange &&) = delete;
5252
~NormalLaneChange() override = default;
5353

54+
void update_lanes(const bool is_approved) final;
55+
5456
void updateLaneChangeStatus() override;
5557

5658
std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const override;
@@ -105,8 +107,6 @@ class NormalLaneChange : public LaneChangeBase
105107
TurnSignalInfo get_current_turn_signal_info() override;
106108

107109
protected:
108-
lanelet::ConstLanelets getCurrentLanes() const override;
109-
110110
lanelet::ConstLanelets getLaneChangeLanes(
111111
const lanelet::ConstLanelets & current_lanes, Direction direction) const override;
112112

@@ -124,9 +124,7 @@ class NormalLaneChange : public LaneChangeBase
124124
const LaneChangeLanesFilteredObjects & predicted_objects,
125125
const lanelet::ConstLanelets & current_lanes) const;
126126

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

131129
void filterOncomingObjects(PredictedObjects & objects) const;
132130

@@ -203,6 +201,11 @@ class NormalLaneChange : public LaneChangeBase
203201

204202
double getStopTime() const { return stop_time_; }
205203

204+
const lanelet::ConstLanelets & get_target_lanes() const
205+
{
206+
return common_data_ptr_->lanes_ptr->target;
207+
}
208+
206209
double stop_time_{0.0};
207210
};
208211
} // namespace autoware::behavior_path_planner

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp

+17-2
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,8 @@ class LaneChangeBase
6565
LaneChangeBase & operator=(LaneChangeBase &&) = delete;
6666
virtual ~LaneChangeBase() = default;
6767

68+
virtual void update_lanes(const bool is_approved) = 0;
69+
6870
virtual void updateLaneChangeStatus() = 0;
6971

7072
virtual std::pair<bool, bool> getSafePath(LaneChangePath & safe_path) const = 0;
@@ -139,6 +141,11 @@ class LaneChangeBase
139141

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

144+
const lanelet::ConstLanelets & get_current_lanes() const
145+
{
146+
return common_data_ptr_->lanes_ptr->current;
147+
}
148+
142149
const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; }
143150

144151
LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; }
@@ -163,9 +170,19 @@ class LaneChangeBase
163170
common_data_ptr_->bpp_param_ptr =
164171
std::make_shared<BehaviorPathPlannerParameters>(data->parameters);
165172
}
173+
174+
if (!common_data_ptr_->lanes_ptr) {
175+
common_data_ptr_->lanes_ptr = std::make_shared<lane_change::Lanes>();
176+
}
177+
178+
if (!common_data_ptr_->lanes_polygon_ptr) {
179+
common_data_ptr_->lanes_polygon_ptr = std::make_shared<lane_change::LanesPolygon>();
180+
}
181+
166182
common_data_ptr_->self_odometry_ptr = data->self_odometry;
167183
common_data_ptr_->route_handler_ptr = data->route_handler;
168184
common_data_ptr_->lc_param_ptr = lane_change_parameters_;
185+
common_data_ptr_->lc_type = type_;
169186
common_data_ptr_->direction = direction_;
170187
}
171188

@@ -211,8 +228,6 @@ class LaneChangeBase
211228
virtual TurnSignalInfo get_current_turn_signal_info() = 0;
212229

213230
protected:
214-
virtual lanelet::ConstLanelets getCurrentLanes() const = 0;
215-
216231
virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0;
217232

218233
virtual PathWithLaneId getPrepareSegment(

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp

+21-19
Original file line numberDiff line numberDiff line change
@@ -183,16 +183,20 @@ struct PhaseInfo
183183
}
184184
};
185185

186+
struct Lanes
187+
{
188+
lanelet::ConstLanelets current;
189+
lanelet::ConstLanelets target;
190+
std::vector<lanelet::ConstLanelets> preceding_target;
191+
};
192+
186193
struct Info
187194
{
188195
PhaseInfo longitudinal_acceleration{0.0, 0.0};
189196
PhaseInfo velocity{0.0, 0.0};
190197
PhaseInfo duration{0.0, 0.0};
191198
PhaseInfo length{0.0, 0.0};
192199

193-
lanelet::ConstLanelets current_lanes{};
194-
lanelet::ConstLanelets target_lanes{};
195-
196200
Pose lane_changing_start{};
197201
Pose lane_changing_end{};
198202

@@ -225,23 +229,26 @@ struct LanesPolygon
225229
{
226230
std::optional<lanelet::BasicPolygon2d> current;
227231
std::optional<lanelet::BasicPolygon2d> target;
228-
std::vector<lanelet::BasicPolygon2d> target_backward;
232+
std::optional<lanelet::BasicPolygon2d> expanded_target;
233+
lanelet::BasicPolygon2d target_neighbor;
234+
std::vector<lanelet::BasicPolygon2d> preceding_target;
229235
};
230236

231-
struct Lanes
232-
{
233-
lanelet::ConstLanelets current;
234-
lanelet::ConstLanelets target;
235-
std::vector<lanelet::ConstLanelets> preceding_target;
236-
};
237+
using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
238+
using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
239+
using LCParamPtr = std::shared_ptr<Parameters>;
240+
using LanesPtr = std::shared_ptr<Lanes>;
241+
using LanesPolygonPtr = std::shared_ptr<LanesPolygon>;
237242

238243
struct CommonData
239244
{
240-
std::shared_ptr<RouteHandler> route_handler_ptr;
245+
RouteHandlerPtr route_handler_ptr;
241246
Odometry::ConstSharedPtr self_odometry_ptr;
242-
std::shared_ptr<BehaviorPathPlannerParameters> bpp_param_ptr;
243-
std::shared_ptr<Parameters> lc_param_ptr;
244-
Lanes lanes;
247+
BppParamPtr bpp_param_ptr;
248+
LCParamPtr lc_param_ptr;
249+
LanesPtr lanes_ptr;
250+
LanesPolygonPtr lanes_polygon_ptr;
251+
ModuleType lc_type;
245252
Direction direction;
246253

247254
[[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; }
@@ -259,12 +266,7 @@ struct CommonData
259266
return std::hypot(x, y);
260267
}
261268
};
262-
263-
using RouteHandlerPtr = std::shared_ptr<RouteHandler>;
264-
using BppParamPtr = std::shared_ptr<BehaviorPathPlannerParameters>;
265-
using LCParamPtr = std::shared_ptr<Parameters>;
266269
using CommonDataPtr = std::shared_ptr<CommonData>;
267-
using LanesPtr = std::shared_ptr<Lanes>;
268270
} // namespace autoware::behavior_path_planner::lane_change
269271

270272
namespace autoware::behavior_path_planner

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp

+12-12
Original file line numberDiff line numberDiff line change
@@ -23,31 +23,31 @@
2323

2424
#include <vector>
2525

26-
namespace autoware::behavior_path_planner
26+
namespace autoware::behavior_path_planner::lane_change
2727
{
2828
using autoware::behavior_path_planner::TurnSignalInfo;
2929
using tier4_planning_msgs::msg::PathWithLaneId;
30-
struct LaneChangePath
30+
struct Path
3131
{
3232
PathWithLaneId path{};
3333
ShiftedPath shifted_path{};
34-
LaneChangeInfo info;
35-
bool is_safe{false};
34+
Info info{};
3635
};
37-
using LaneChangePaths = std::vector<LaneChangePath>;
3836

39-
struct LaneChangeStatus
37+
struct Status
4038
{
41-
PathWithLaneId lane_follow_path{};
42-
LaneChangePath lane_change_path{};
43-
lanelet::ConstLanelets current_lanes{};
44-
lanelet::ConstLanelets target_lanes{};
45-
std::vector<lanelet::Id> lane_follow_lane_ids{};
46-
std::vector<lanelet::Id> lane_change_lane_ids{};
39+
Path lane_change_path{};
4740
bool is_safe{false};
4841
bool is_valid_path{false};
4942
double start_distance{0.0};
5043
};
5144

45+
} // namespace autoware::behavior_path_planner::lane_change
46+
47+
namespace autoware::behavior_path_planner
48+
{
49+
using LaneChangePath = lane_change::Path;
50+
using LaneChangePaths = std::vector<lane_change::Path>;
51+
using LaneChangeStatus = lane_change::Status;
5252
} // namespace autoware::behavior_path_planner
5353
#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp

+13-9
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,7 @@ using autoware::universe_utils::Polygon2d;
4848
using autoware_perception_msgs::msg::PredictedObject;
4949
using autoware_perception_msgs::msg::PredictedObjects;
5050
using autoware_perception_msgs::msg::PredictedPath;
51+
using behavior_path_planner::lane_change::CommonDataPtr;
5152
using behavior_path_planner::lane_change::LanesPolygon;
5253
using behavior_path_planner::lane_change::PathSafetyStatus;
5354
using geometry_msgs::msg::Point;
@@ -114,8 +115,9 @@ bool isPathInLanelets(
114115
const lanelet::ConstLanelets & target_lanes);
115116

116117
std::optional<LaneChangePath> constructCandidatePath(
117-
const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment,
118-
const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path,
118+
const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info,
119+
const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
120+
const PathWithLaneId & target_lane_reference_path,
119121
const std::vector<std::vector<int64_t>> & sorted_lane_ids);
120122

121123
ShiftLine getLaneChangingShiftLine(
@@ -177,10 +179,9 @@ bool isParkedObject(
177179
const double ratio_threshold);
178180

179181
bool passParkedObject(
180-
const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
182+
const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path,
181183
const std::vector<ExtendedPredictedObject> & objects, const double minimum_lane_change_length,
182-
const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters,
183-
CollisionCheckDebugMap & object_debug);
184+
const bool is_goal_in_route, CollisionCheckDebugMap & object_debug);
184185

185186
std::optional<size_t> getLeadingStaticObjectIdx(
186187
const RouteHandler & route_handler, const LaneChangePath & lane_change_path,
@@ -195,7 +196,8 @@ ExtendedPredictedObject transform(
195196
const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase);
196197

197198
bool isCollidedPolygonsInLanelet(
198-
const std::vector<Polygon2d> & collided_polygons, const lanelet::ConstLanelets & lanes);
199+
const std::vector<Polygon2d> & collided_polygons,
200+
const std::optional<lanelet::BasicPolygon2d> & lanes_polygon);
199201

200202
/**
201203
* @brief Generates expanded lanelets based on the given direction and offsets.
@@ -295,9 +297,11 @@ double calcPhaseLength(
295297
const double velocity, const double maximum_velocity, const double acceleration,
296298
const double time);
297299

298-
LanesPolygon createLanesPolygon(
299-
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
300-
const std::vector<lanelet::ConstLanelets> & target_backward_lanes);
300+
LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr);
301+
302+
bool is_same_lane_with_prev_iteration(
303+
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
304+
const lanelet::ConstLanelets & target_lanes);
301305
} // namespace autoware::behavior_path_planner::utils::lane_change
302306

303307
namespace autoware::behavior_path_planner::utils::lane_change::debug

‎planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,7 @@ void LaneChangeInterface::updateData()
7575
{
7676
universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper());
7777
module_type_->setPreviousModuleOutput(getPreviousModuleOutput());
78+
module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING);
7879
module_type_->updateSpecialData();
7980

8081
if (isWaitingApproval() || module_type_->isAbortState()) {
@@ -136,7 +137,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
136137
*prev_approved_path_ = getPreviousModuleOutput().path;
137138

138139
BehaviorModuleOutput out = getPreviousModuleOutput();
139-
module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path);
140+
module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path);
140141
out.turn_signal_info = module_type_->get_current_turn_signal_info();
141142

142143
const auto & lane_change_debug = module_type_->getDebugData();

0 commit comments

Comments
 (0)