@@ -195,7 +195,6 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
195
195
{
196
196
lanelet::ConstLanelets route_lanelets;
197
197
lanelet::ConstLanelets end_lanelets;
198
- lanelet::ConstLanelets normal_lanelets;
199
198
lanelet::ConstLanelets goal_lanelets;
200
199
201
200
for (const auto & route_section : route.segments ) {
@@ -210,16 +209,14 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
210
209
}
211
210
}
212
211
213
- std_msgs::msg::ColorRGBA cl_route;
214
- std_msgs::msg::ColorRGBA cl_ll_borders;
215
- std_msgs::msg::ColorRGBA cl_end;
216
- std_msgs::msg::ColorRGBA cl_normal;
217
- std_msgs::msg::ColorRGBA cl_goal;
218
- set_color (&cl_route, 0.8 , 0.99 , 0.8 , 0.15 );
219
- set_color (&cl_goal, 0.2 , 0.4 , 0.4 , 0.05 );
220
- set_color (&cl_end, 0.2 , 0.2 , 0.4 , 0.05 );
221
- set_color (&cl_normal, 0.2 , 0.4 , 0.2 , 0.05 );
222
- set_color (&cl_ll_borders, 1.0 , 1.0 , 1.0 , 0.999 );
212
+ const std_msgs::msg::ColorRGBA cl_route =
213
+ tier4_autoware_utils::createMarkerColor (0.8 , 0.99 , 0.8 , 0.15 );
214
+ const std_msgs::msg::ColorRGBA cl_ll_borders =
215
+ tier4_autoware_utils::createMarkerColor (0.2 , 0.4 , 0.4 , 0.05 );
216
+ const std_msgs::msg::ColorRGBA cl_end =
217
+ tier4_autoware_utils::createMarkerColor (0.2 , 0.2 , 0.4 , 0.05 );
218
+ const std_msgs::msg::ColorRGBA cl_goal =
219
+ tier4_autoware_utils::createMarkerColor (1.0 , 1.0 , 1.0 , 0.999 );
223
220
224
221
visualization_msgs::msg::MarkerArray route_marker_array;
225
222
insert_marker_array (
@@ -231,9 +228,6 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
231
228
insert_marker_array (
232
229
&route_marker_array,
233
230
lanelet::visualization::laneletsAsTriangleMarkerArray (" end_lanelets" , end_lanelets, cl_end));
234
- insert_marker_array (
235
- &route_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray (
236
- " normal_lanelets" , normal_lanelets, cl_normal));
237
231
insert_marker_array (
238
232
&route_marker_array,
239
233
lanelet::visualization::laneletsAsTriangleMarkerArray (" goal_lanelets" , goal_lanelets, cl_goal));
@@ -270,23 +264,18 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
270
264
return msg;
271
265
}
272
266
273
- bool DefaultPlanner::check_goal_footprint (
267
+ bool DefaultPlanner::check_goal_footprint_inside_lanes (
274
268
const lanelet::ConstLanelet & current_lanelet,
275
269
const lanelet::ConstLanelet & combined_prev_lanelet,
276
270
const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
277
271
const double search_margin)
278
272
{
279
- std::vector<tier4_autoware_utils::Point2d> points_intersection;
280
-
281
273
// check if goal footprint is in current lane
282
- boost::geometry::intersection (
283
- goal_footprint, combined_prev_lanelet.polygon2d ().basicPolygon (), points_intersection);
284
- if (points_intersection.empty ()) {
274
+ if (boost::geometry::within (goal_footprint, combined_prev_lanelet.polygon2d ().basicPolygon ())) {
285
275
return true ;
286
276
}
287
- points_intersection.clear ();
288
277
289
- // check if goal footprint is in between many lanelets
278
+ // check if goal footprint is in between many lanelets in depth-first search manner
290
279
for (const auto & next_lane : routing_graph_ptr_->following (current_lanelet)) {
291
280
next_lane_length += lanelet::utils::getLaneletLength2d (next_lane);
292
281
lanelet::ConstLanelets lanelets;
@@ -295,17 +284,20 @@ bool DefaultPlanner::check_goal_footprint(
295
284
lanelet::ConstLanelet combined_lanelets =
296
285
combine_lanelets_with_shoulder (lanelets, shoulder_lanelets_);
297
286
298
- // if next lanelet length longer than vehicle longitudinal offset
287
+ // if next lanelet length is longer than vehicle longitudinal offset
299
288
if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
300
289
next_lane_length -= lanelet::utils::getLaneletLength2d (next_lane);
301
- boost::geometry::intersection (
302
- goal_footprint, combined_lanelets. polygon2d (). basicPolygon (), points_intersection);
303
- if (points_intersection. empty ( )) {
290
+ // and if the goal_footprint is within the (accumulated) combined_lanelets, terminate the
291
+ // query
292
+ if (boost::geometry::within (goal_footprint, combined_lanelets. polygon2d (). basicPolygon () )) {
304
293
return true ;
305
294
}
306
- points_intersection.clear ();
307
- } else { // if next lanelet length shorter than vehicle longitudinal offset -> recursive call
308
- if (!check_goal_footprint (next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
295
+ // if not, iteration continues to next next_lane, and this subtree is terminated
296
+ } else { // if next lanelet length is shorter than vehicle longitudinal offset, check the
297
+ // overlap with the polygon including the next_lane(s) until the additional lanes get
298
+ // longer than ego vehicle length
299
+ if (!check_goal_footprint_inside_lanes (
300
+ next_lane, combined_lanelets, goal_footprint, next_lane_length)) {
309
301
next_lane_length -= lanelet::utils::getLaneletLength2d (next_lane);
310
302
continue ;
311
303
} else {
@@ -352,13 +344,13 @@ bool DefaultPlanner::is_goal_valid(
352
344
353
345
double next_lane_length = 0.0 ;
354
346
// combine calculated route lanelets
355
- lanelet::ConstLanelet combined_prev_lanelet =
347
+ const lanelet::ConstLanelet combined_prev_lanelet =
356
348
combine_lanelets_with_shoulder (path_lanelets, shoulder_lanelets_);
357
349
358
350
// check if goal footprint exceeds lane when the goal isn't in parking_lot
359
351
if (
360
352
param_.check_footprint_inside_lanes &&
361
- !check_goal_footprint (
353
+ !check_goal_footprint_inside_lanes (
362
354
closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
363
355
!is_in_parking_lot (
364
356
lanelet::utils::query::getAllParkingLots (lanelet_map_ptr_),
0 commit comments