Skip to content

Commit c866b21

Browse files
soblinkarishma1911
authored andcommitted
refactor(mission_planner): add comment functions/remove unused stuff/add comment (autowarefoundation#6447)
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
1 parent a738e46 commit c866b21

File tree

5 files changed

+62
-49
lines changed

5 files changed

+62
-49
lines changed

planning/mission_planner/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
<maintainer email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</maintainer>
1010
<maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
1111
<maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
12+
<maintainer email="mamoru.sobue@tier4.jp">Mamoru Sobue</maintainer>
1213
<license>Apache License 2.0</license>
1314

1415
<author email="ryohsuke.mitsudome@tier4.jp">Ryohsuke Mitsudome</author>

planning/mission_planner/src/lanelet2_plugins/default_planner.cpp

+23-31
Original file line numberDiff line numberDiff line change
@@ -195,7 +195,6 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
195195
{
196196
lanelet::ConstLanelets route_lanelets;
197197
lanelet::ConstLanelets end_lanelets;
198-
lanelet::ConstLanelets normal_lanelets;
199198
lanelet::ConstLanelets goal_lanelets;
200199

201200
for (const auto & route_section : route.segments) {
@@ -210,16 +209,14 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
210209
}
211210
}
212211

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);
223220

224221
visualization_msgs::msg::MarkerArray route_marker_array;
225222
insert_marker_array(
@@ -231,9 +228,6 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route)
231228
insert_marker_array(
232229
&route_marker_array,
233230
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));
237231
insert_marker_array(
238232
&route_marker_array,
239233
lanelet::visualization::laneletsAsTriangleMarkerArray("goal_lanelets", goal_lanelets, cl_goal));
@@ -270,23 +264,18 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint(
270264
return msg;
271265
}
272266

273-
bool DefaultPlanner::check_goal_footprint(
267+
bool DefaultPlanner::check_goal_footprint_inside_lanes(
274268
const lanelet::ConstLanelet & current_lanelet,
275269
const lanelet::ConstLanelet & combined_prev_lanelet,
276270
const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
277271
const double search_margin)
278272
{
279-
std::vector<tier4_autoware_utils::Point2d> points_intersection;
280-
281273
// 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())) {
285275
return true;
286276
}
287-
points_intersection.clear();
288277

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
290279
for (const auto & next_lane : routing_graph_ptr_->following(current_lanelet)) {
291280
next_lane_length += lanelet::utils::getLaneletLength2d(next_lane);
292281
lanelet::ConstLanelets lanelets;
@@ -295,17 +284,20 @@ bool DefaultPlanner::check_goal_footprint(
295284
lanelet::ConstLanelet combined_lanelets =
296285
combine_lanelets_with_shoulder(lanelets, shoulder_lanelets_);
297286

298-
// if next lanelet length longer than vehicle longitudinal offset
287+
// if next lanelet length is longer than vehicle longitudinal offset
299288
if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) {
300289
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())) {
304293
return true;
305294
}
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)) {
309301
next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane);
310302
continue;
311303
} else {
@@ -352,13 +344,13 @@ bool DefaultPlanner::is_goal_valid(
352344

353345
double next_lane_length = 0.0;
354346
// combine calculated route lanelets
355-
lanelet::ConstLanelet combined_prev_lanelet =
347+
const lanelet::ConstLanelet combined_prev_lanelet =
356348
combine_lanelets_with_shoulder(path_lanelets, shoulder_lanelets_);
357349

358350
// check if goal footprint exceeds lane when the goal isn't in parking_lot
359351
if (
360352
param_.check_footprint_inside_lanes &&
361-
!check_goal_footprint(
353+
!check_goal_footprint_inside_lanes(
362354
closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) &&
363355
!is_in_parking_lot(
364356
lanelet::utils::query::getAllParkingLots(lanelet_map_ptr_),

planning/mission_planner/src/lanelet2_plugins/default_planner.hpp

+26-1
Original file line numberDiff line numberDiff line change
@@ -71,13 +71,38 @@ class DefaultPlanner : public mission_planner::PlannerPlugin
7171

7272
void initialize_common(rclcpp::Node * node);
7373
void map_callback(const HADMapBin::ConstSharedPtr msg);
74-
bool check_goal_footprint(
74+
75+
/**
76+
* @brief check if the goal_footprint is within the combined lanelet of route_lanelets plus the
77+
* succeeding lanelets around the goal
78+
* @attention this function will terminate when the accumulated search length from the initial
79+
* current_lanelet exceeds max_longitudinal_offset_m + search_margin, so under normal assumptions
80+
* (i.e. the map is composed of finite elements of practically normal sized lanelets), it is
81+
* assured to terminate
82+
* @param current_lanelet the start lanelet to begin recursive query
83+
* @param combined_prev_lanelet initial entire route_lanelets plus the small consecutive lanelets
84+
* around the goal during the query
85+
* @param next_lane_length the accumulated total length from the start lanelet of the search to
86+
* the lanelet of current goal query
87+
*/
88+
bool check_goal_footprint_inside_lanes(
7589
const lanelet::ConstLanelet & current_lanelet,
7690
const lanelet::ConstLanelet & combined_prev_lanelet,
7791
const tier4_autoware_utils::Polygon2d & goal_footprint, double & next_lane_length,
7892
const double search_margin = 2.0);
93+
94+
/**
95+
* @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the
96+
* footprint around the goal does not overlap the lanes
97+
*/
7998
bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets);
99+
100+
/**
101+
* @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of
102+
* route_sections) and return the z-aligned goal position
103+
*/
80104
Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections);
105+
81106
void updateRoute(const PlannerPlugin::LaneletRoute & route);
82107
void clearRoute();
83108
};

planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp

+5-14
Original file line numberDiff line numberDiff line change
@@ -21,11 +21,6 @@
2121
#include <unordered_set>
2222
#include <utility>
2323

24-
bool exists(const std::unordered_set<lanelet::Id> & set, const lanelet::Id & id)
25-
{
26-
return set.find(id) != set.end();
27-
}
28-
2924
tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
3025
tier4_autoware_utils::LinearRing2d footprint)
3126
{
@@ -40,14 +35,6 @@ tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
4035
return footprint_polygon;
4136
}
4237

43-
void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a)
44-
{
45-
cl->r = r;
46-
cl->g = g;
47-
cl->b = b;
48-
cl->a = a;
49-
}
50-
5138
void insert_marker_array(
5239
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2)
5340
{
@@ -64,7 +51,7 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
6451
std::vector<uint64_t> right_bound_ids;
6552

6653
for (const auto & llt : lanelets) {
67-
if (llt.id() != 0) {
54+
if (llt.id() != lanelet::InvalId) {
6855
left_bound_ids.push_back(llt.leftBound().id());
6956
right_bound_ids.push_back(llt.rightBound().id());
7057
}
@@ -96,6 +83,8 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
9683
add_bound(left_shared_shoulder_it->leftBound(), lefts);
9784
} else if (
9885
// if not exist, add left bound of lanelet to lefts
86+
// if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`,
87+
// then its left bound constitutes the left boundary of the entire merged lanelet
9988
std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) {
10089
add_bound(llt.leftBound(), lefts);
10190
}
@@ -108,6 +97,8 @@ lanelet::ConstLanelet combine_lanelets_with_shoulder(
10897
add_bound(right_shared_shoulder_it->rightBound(), rights);
10998
} else if (
11099
// if not exist, add right bound of lanelet to rights
100+
// if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`,
101+
// then its right bound constitutes the right boundary of the entire merged lanelet
111102
std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) {
112103
add_bound(llt.rightBound(), rights);
113104
}

planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp

+7-3
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,6 @@
3030
#include <unordered_set>
3131
#include <vector>
3232

33-
bool exists(const std::unordered_set<lanelet::Id> & set, const lanelet::Id & id);
34-
3533
template <typename T>
3634
bool exists(const std::vector<T> & vectors, const T & item)
3735
{
@@ -45,12 +43,18 @@ bool exists(const std::vector<T> & vectors, const T & item)
4543

4644
tier4_autoware_utils::Polygon2d convert_linear_ring_to_polygon(
4745
tier4_autoware_utils::LinearRing2d footprint);
48-
void set_color(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a);
4946
void insert_marker_array(
5047
visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2);
5148

49+
/**
50+
* @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost
51+
* bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively
52+
* @param lanelets topologically sorted sequence of lanelets
53+
* @param shoulder_lanelets list of possible road_shoulder lanelets to combine with lanelets
54+
*/
5255
lanelet::ConstLanelet combine_lanelets_with_shoulder(
5356
const lanelet::ConstLanelets & lanelets, const lanelet::ConstLanelets & shoulder_lanelets);
57+
5458
std::vector<geometry_msgs::msg::Point> convertCenterlineToPoints(const lanelet::Lanelet & lanelet);
5559
geometry_msgs::msg::Pose convertBasicPoint3dToPose(
5660
const lanelet::BasicPoint3d & point, const double lane_yaw);

0 commit comments

Comments
 (0)